src/jtag/drivers/mpsse.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /**************************************************************************
   4  *   Copyright (C) 2012 by Andreas Fritiofson                              *
   5  *   andreas.fritiofson@gmail.com                                          *
   6  ***************************************************************************/
   7
   8 #ifdef HAVE_CONFIG_H
   9 #include "config.h"
  10 #endif
  11
  12 #include "mpsse.h"
  13 #include "helper/log.h"
  14 #include "helper/replacements.h"
  15 #include "helper/time_support.h"
  16 #include "libusb_helper.h"
  17 #include <libusb.h>
  18
  19 /* Compatibility define for older libusb-1.0 */
  20 #ifndef LIBUSB_CALL
  21 #define LIBUSB_CALL
  22 #endif
  23
  24 #define DEBUG_PRINT_BUF(buf, len) \
  25         do { \
  26                 if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) { \
  27                         char buf_string[32 * 3 + 1]; \
  28                         int buf_string_pos = 0; \
  29                         for (int i = 0; i < len; i++) { \
  30                                 buf_string_pos += sprintf(buf_string + buf_string_pos, " %02x", buf[i]); \
  31                                 if (i % 32 == 32 - 1) { \
  32                                         LOG_DEBUG_IO("%s", buf_string); \
  33                                         buf_string_pos = 0; \
  34                                 } \
  35                         } \
  36                         if (buf_string_pos > 0) \
  37                                 LOG_DEBUG_IO("%s", buf_string);\
  38                 } \
  39         } while (0)
  40
  41 #define FTDI_DEVICE_OUT_REQTYPE (LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE)
  42 #define FTDI_DEVICE_IN_REQTYPE (0x80 | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE)
  43
  44 #define BITMODE_MPSSE 0x02
  45
  46 #define SIO_RESET_REQUEST             0x00
  47 #define SIO_SET_LATENCY_TIMER_REQUEST 0x09
  48 #define SIO_GET_LATENCY_TIMER_REQUEST 0x0A
  49 #define SIO_SET_BITMODE_REQUEST       0x0B
  50
  51 #define SIO_RESET_SIO 0
  52 #define SIO_RESET_PURGE_RX 1
  53 #define SIO_RESET_PURGE_TX 2
  54
  55 struct mpsse_ctx {
  56         struct libusb_context *usb_ctx;
  57         struct libusb_device_handle *usb_dev;
  58         unsigned int usb_write_timeout;
  59         unsigned int usb_read_timeout;
  60         uint8_t in_ep;
  61         uint8_t out_ep;
  62         uint16_t max_packet_size;
  63         uint16_t index;
  64         uint8_t interface;
  65         enum ftdi_chip_type type;
  66         uint8_t *write_buffer;
  67         unsigned write_size;
  68         unsigned write_count;
  69         uint8_t *read_buffer;
  70         unsigned read_size;
  71         unsigned read_count;
  72         uint8_t *read_chunk;
  73         unsigned read_chunk_size;
  74         struct bit_copy_queue read_queue;
  75         int retval;
  76 };
  77
  78 /* Returns true if the string descriptor indexed by str_index in device matches string */
  79 static bool string_descriptor_equal(struct libusb_device_handle *device, uint8_t str_index,
  80         const char *string)
  81 {
  82         int retval;
  83         char desc_string[256]; /* Max size of string descriptor */
  84         retval = libusb_get_string_descriptor_ascii(device, str_index, (unsigned char *)desc_string,
  85                         sizeof(desc_string));
  86         if (retval < 0) {
  87                 LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %s", libusb_error_name(retval));
  88                 return false;
  89         }
  90         return strncmp(string, desc_string, sizeof(desc_string)) == 0;
  91 }
  92
  93 static bool device_location_equal(struct libusb_device *device, const char *location)
  94 {
  95         bool result = false;
  96 #ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
  97         char *loc = strdup(location);
  98         uint8_t port_path[7];
  99         int path_step, path_len;
 100         uint8_t dev_bus = libusb_get_bus_number(device);
 101         char *ptr;
 102
 103         path_len = libusb_get_port_numbers(device, port_path, 7);
 104         if (path_len == LIBUSB_ERROR_OVERFLOW) {
 105                 LOG_ERROR("cannot determine path to usb device! (more than 7 ports in path)");
 106                 goto done;
 107         }
 108
 109         LOG_DEBUG("device path has %i steps", path_len);
 110
 111         ptr = strtok(loc, "-:");
 112         if (!ptr) {
 113                 LOG_DEBUG("no ':' in path");
 114                 goto done;
 115         }
 116         if (atoi(ptr) != dev_bus) {
 117                 LOG_DEBUG("bus mismatch");
 118                 goto done;
 119         }
 120
 121         path_step = 0;
 122         while (path_step < 7) {
 123                 ptr = strtok(NULL, ".,");
 124                 if (!ptr) {
 125                         LOG_DEBUG("no more tokens in path at step %i", path_step);
 126                         break;
 127                 }
 128
 129                 if (path_step < path_len
 130                         && atoi(ptr) != port_path[path_step]) {
 131                         LOG_DEBUG("path mismatch at step %i", path_step);
 132                         break;
 133                 }
 134
 135                 path_step++;
 136         };
 137
 138         /* walked the full path, all elements match */
 139         if (path_step == path_len)
 140                 result = true;
 141
 142  done:
 143         free(loc);
 144 #endif
 145         return result;
 146 }
 147
 148 /* Helper to open a libusb device that matches vid, pid, product string and/or serial string.
 149  * Set any field to 0 as a wildcard. If the device is found true is returned, with ctx containing
 150  * the already opened handle. ctx->interface must be set to the desired interface (channel) number
 151  * prior to calling this function. */
 152 static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t vids[], const uint16_t pids[],
 153         const char *product, const char *serial, const char *location)
 154 {
 155         struct libusb_device **list;
 156         struct libusb_device_descriptor desc;
 157         struct libusb_config_descriptor *config0;
 158         int err;
 159         bool found = false;
 160         ssize_t cnt = libusb_get_device_list(ctx->usb_ctx, &list);
 161         if (cnt < 0)
 162                 LOG_ERROR("libusb_get_device_list() failed with %s", libusb_error_name(cnt));
 163
 164         for (ssize_t i = 0; i < cnt; i++) {
 165                 struct libusb_device *device = list[i];
 166
 167                 err = libusb_get_device_descriptor(device, &desc);
 168                 if (err != LIBUSB_SUCCESS) {
 169                         LOG_ERROR("libusb_get_device_descriptor() failed with %s", libusb_error_name(err));
 170                         continue;
 171                 }
 172
 173                 if (!jtag_libusb_match_ids(&desc, vids, pids))
 174                         continue;
 175
 176                 err = libusb_open(device, &ctx->usb_dev);
 177                 if (err != LIBUSB_SUCCESS) {
 178                         LOG_ERROR("libusb_open() failed with %s",
 179                                   libusb_error_name(err));
 180                         continue;
 181                 }
 182
 183                 if (location && !device_location_equal(device, location)) {
 184                         libusb_close(ctx->usb_dev);
 185                         continue;
 186                 }
 187
 188                 if (product && !string_descriptor_equal(ctx->usb_dev, desc.iProduct, product)) {
 189                         libusb_close(ctx->usb_dev);
 190                         continue;
 191                 }
 192
 193                 if (serial && !string_descriptor_equal(ctx->usb_dev, desc.iSerialNumber, serial)) {
 194                         libusb_close(ctx->usb_dev);
 195                         continue;
 196                 }
 197
 198                 found = true;
 199                 break;
 200         }
 201
 202         libusb_free_device_list(list, 1);
 203
 204         if (!found) {
 205                 /* The caller reports detailed error desc */
 206                 return false;
 207         }
 208
 209         err = libusb_get_config_descriptor(libusb_get_device(ctx->usb_dev), 0, &config0);
 210         if (err != LIBUSB_SUCCESS) {
 211                 LOG_ERROR("libusb_get_config_descriptor() failed with %s", libusb_error_name(err));
 212                 libusb_close(ctx->usb_dev);
 213                 return false;
 214         }
 215
 216         /* Make sure the first configuration is selected */
 217         int cfg;
 218         err = libusb_get_configuration(ctx->usb_dev, &cfg);
 219         if (err != LIBUSB_SUCCESS) {
 220                 LOG_ERROR("libusb_get_configuration() failed with %s", libusb_error_name(err));
 221                 goto error;
 222         }
 223
 224         if (desc.bNumConfigurations > 0 && cfg != config0->bConfigurationValue) {
 225                 err = libusb_set_configuration(ctx->usb_dev, config0->bConfigurationValue);
 226                 if (err != LIBUSB_SUCCESS) {
 227                         LOG_ERROR("libusb_set_configuration() failed with %s", libusb_error_name(err));
 228                         goto error;
 229                 }
 230         }
 231
 232         /* Try to detach ftdi_sio kernel module */
 233         err = libusb_detach_kernel_driver(ctx->usb_dev, ctx->interface);
 234         if (err != LIBUSB_SUCCESS && err != LIBUSB_ERROR_NOT_FOUND
 235                         && err != LIBUSB_ERROR_NOT_SUPPORTED) {
 236                 LOG_WARNING("libusb_detach_kernel_driver() failed with %s, trying to continue anyway",
 237                         libusb_error_name(err));
 238         }
 239
 240         err = libusb_claim_interface(ctx->usb_dev, ctx->interface);
 241         if (err != LIBUSB_SUCCESS) {
 242                 LOG_ERROR("libusb_claim_interface() failed with %s", libusb_error_name(err));
 243                 goto error;
 244         }
 245
 246         /* Reset FTDI device */
 247         err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE,
 248                         SIO_RESET_REQUEST, SIO_RESET_SIO,
 249                         ctx->index, NULL, 0, ctx->usb_write_timeout);
 250         if (err < 0) {
 251                 LOG_ERROR("failed to reset FTDI device: %s", libusb_error_name(err));
 252                 goto error;
 253         }
 254
 255         switch (desc.bcdDevice) {
 256         case 0x500:
 257                 ctx->type = TYPE_FT2232C;
 258                 break;
 259         case 0x700:
 260                 ctx->type = TYPE_FT2232H;
 261                 break;
 262         case 0x800:
 263                 ctx->type = TYPE_FT4232H;
 264                 break;
 265         case 0x900:
 266                 ctx->type = TYPE_FT232H;
 267                 break;
 268         default:
 269                 LOG_ERROR("unsupported FTDI chip type: 0x%04x", desc.bcdDevice);
 270                 goto error;
 271         }
 272
 273         /* Determine maximum packet size and endpoint addresses */
 274         if (!(desc.bNumConfigurations > 0 && ctx->interface < config0->bNumInterfaces
 275                         && config0->interface[ctx->interface].num_altsetting > 0))
 276                 goto desc_error;
 277
 278         const struct libusb_interface_descriptor *descriptor;
 279         descriptor = &config0->interface[ctx->interface].altsetting[0];
 280         if (descriptor->bNumEndpoints != 2)
 281                 goto desc_error;
 282
 283         ctx->in_ep = 0;
 284         ctx->out_ep = 0;
 285         for (int i = 0; i < descriptor->bNumEndpoints; i++) {
 286                 if (descriptor->endpoint[i].bEndpointAddress & 0x80) {
 287                         ctx->in_ep = descriptor->endpoint[i].bEndpointAddress;
 288                         ctx->max_packet_size =
 289                                         descriptor->endpoint[i].wMaxPacketSize;
 290                 } else {
 291                         ctx->out_ep = descriptor->endpoint[i].bEndpointAddress;
 292                 }
 293         }
 294
 295         if (ctx->in_ep == 0 || ctx->out_ep == 0)
 296                 goto desc_error;
 297
 298         libusb_free_config_descriptor(config0);
 299         return true;
 300
 301 desc_error:
 302         LOG_ERROR("unrecognized USB device descriptor");
 303 error:
 304         libusb_free_config_descriptor(config0);
 305         libusb_close(ctx->usb_dev);
 306         return false;
 307 }
 308
 309 struct mpsse_ctx *mpsse_open(const uint16_t vids[], const uint16_t pids[], const char *description,
 310         const char *serial, const char *location, int channel)
 311 {
 312         struct mpsse_ctx *ctx = calloc(1, sizeof(*ctx));
 313         int err;
 314
 315         if (!ctx)
 316                 return NULL;
 317
 318         bit_copy_queue_init(&ctx->read_queue);
 319         ctx->read_chunk_size = 16384;
 320         ctx->read_size = 16384;
 321         ctx->write_size = 16384;
 322         ctx->read_chunk = malloc(ctx->read_chunk_size);
 323         ctx->read_buffer = malloc(ctx->read_size);
 324
 325         /* Use calloc to make valgrind happy: buffer_write() sets payload
 326          * on bit basis, so some bits can be left uninitialized in write_buffer.
 327          * Although this is perfectly ok with MPSSE, valgrind reports
 328          * Syscall param ioctl(USBDEVFS_SUBMITURB).buffer points to uninitialised byte(s) */
 329         ctx->write_buffer = calloc(1, ctx->write_size);
 330
 331         if (!ctx->read_chunk || !ctx->read_buffer || !ctx->write_buffer)
 332                 goto error;
 333
 334         ctx->interface = channel;
 335         ctx->index = channel + 1;
 336         ctx->usb_read_timeout = 5000;
 337         ctx->usb_write_timeout = 5000;
 338
 339         err = libusb_init(&ctx->usb_ctx);
 340         if (err != LIBUSB_SUCCESS) {
 341                 LOG_ERROR("libusb_init() failed with %s", libusb_error_name(err));
 342                 goto error;
 343         }
 344
 345         if (!open_matching_device(ctx, vids, pids, description, serial, location)) {
 346                 LOG_ERROR("unable to open ftdi device with description '%s', "
 347                                 "serial '%s' at bus location '%s'",
 348                                 description ? description : "*",
 349                                 serial ? serial : "*",
 350                                 location ? location : "*");
 351                 ctx->usb_dev = NULL;
 352                 goto error;
 353         }
 354
 355         err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE,
 356                         SIO_SET_LATENCY_TIMER_REQUEST, 255, ctx->index, NULL, 0,
 357                         ctx->usb_write_timeout);
 358         if (err < 0) {
 359                 LOG_ERROR("unable to set latency timer: %s", libusb_error_name(err));
 360                 goto error;
 361         }
 362
 363         err = libusb_control_transfer(ctx->usb_dev,
 364                         FTDI_DEVICE_OUT_REQTYPE,
 365                         SIO_SET_BITMODE_REQUEST,
 366                         0x0b | (BITMODE_MPSSE << 8),
 367                         ctx->index,
 368                         NULL,
 369                         0,
 370                         ctx->usb_write_timeout);
 371         if (err < 0) {
 372                 LOG_ERROR("unable to set MPSSE bitmode: %s", libusb_error_name(err));
 373                 goto error;
 374         }
 375
 376         mpsse_purge(ctx);
 377
 378         return ctx;
 379 error:
 380         mpsse_close(ctx);
 381         return NULL;
 382 }
 383
 384 void mpsse_close(struct mpsse_ctx *ctx)
 385 {
 386         if (ctx->usb_dev)
 387                 libusb_close(ctx->usb_dev);
 388         if (ctx->usb_ctx)
 389                 libusb_exit(ctx->usb_ctx);
 390         bit_copy_discard(&ctx->read_queue);
 391
 392         free(ctx->write_buffer);
 393         free(ctx->read_buffer);
 394         free(ctx->read_chunk);
 395         free(ctx);
 396 }
 397
 398 bool mpsse_is_high_speed(struct mpsse_ctx *ctx)
 399 {
 400         return ctx->type != TYPE_FT2232C;
 401 }
 402
 403 void mpsse_purge(struct mpsse_ctx *ctx)
 404 {
 405         int err;
 406         LOG_DEBUG("-");
 407         ctx->write_count = 0;
 408         ctx->read_count = 0;
 409         ctx->retval = ERROR_OK;
 410         bit_copy_discard(&ctx->read_queue);
 411         err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE, SIO_RESET_REQUEST,
 412                         SIO_RESET_PURGE_RX, ctx->index, NULL, 0, ctx->usb_write_timeout);
 413         if (err < 0) {
 414                 LOG_ERROR("unable to purge ftdi rx buffers: %s", libusb_error_name(err));
 415                 return;
 416         }
 417
 418         err = libusb_control_transfer(ctx->usb_dev, FTDI_DEVICE_OUT_REQTYPE, SIO_RESET_REQUEST,
 419                         SIO_RESET_PURGE_TX, ctx->index, NULL, 0, ctx->usb_write_timeout);
 420         if (err < 0) {
 421                 LOG_ERROR("unable to purge ftdi tx buffers: %s", libusb_error_name(err));
 422                 return;
 423         }
 424 }
 425
 426 static unsigned buffer_write_space(struct mpsse_ctx *ctx)
 427 {
 428         /* Reserve one byte for SEND_IMMEDIATE */
 429         return ctx->write_size - ctx->write_count - 1;
 430 }
 431
 432 static unsigned buffer_read_space(struct mpsse_ctx *ctx)
 433 {
 434         return ctx->read_size - ctx->read_count;
 435 }
 436
 437 static void buffer_write_byte(struct mpsse_ctx *ctx, uint8_t data)
 438 {
 439         LOG_DEBUG_IO("%02x", data);
 440         assert(ctx->write_count < ctx->write_size);
 441         ctx->write_buffer[ctx->write_count++] = data;
 442 }
 443
 444 static unsigned buffer_write(struct mpsse_ctx *ctx, const uint8_t *out, unsigned out_offset,
 445         unsigned bit_count)
 446 {
 447         LOG_DEBUG_IO("%d bits", bit_count);
 448         assert(ctx->write_count + DIV_ROUND_UP(bit_count, 8) <= ctx->write_size);
 449         bit_copy(ctx->write_buffer + ctx->write_count, 0, out, out_offset, bit_count);
 450         ctx->write_count += DIV_ROUND_UP(bit_count, 8);
 451         return bit_count;
 452 }
 453
 454 static unsigned buffer_add_read(struct mpsse_ctx *ctx, uint8_t *in, unsigned in_offset,
 455         unsigned bit_count, unsigned offset)
 456 {
 457         LOG_DEBUG_IO("%d bits, offset %d", bit_count, offset);
 458         assert(ctx->read_count + DIV_ROUND_UP(bit_count, 8) <= ctx->read_size);
 459         bit_copy_queued(&ctx->read_queue, in, in_offset, ctx->read_buffer + ctx->read_count, offset,
 460                 bit_count);
 461         ctx->read_count += DIV_ROUND_UP(bit_count, 8);
 462         return bit_count;
 463 }
 464
 465 void mpsse_clock_data_out(struct mpsse_ctx *ctx, const uint8_t *out, unsigned out_offset,
 466         unsigned length, uint8_t mode)
 467 {
 468         mpsse_clock_data(ctx, out, out_offset, NULL, 0, length, mode);
 469 }
 470
 471 void mpsse_clock_data_in(struct mpsse_ctx *ctx, uint8_t *in, unsigned in_offset, unsigned length,
 472         uint8_t mode)
 473 {
 474         mpsse_clock_data(ctx, NULL, 0, in, in_offset, length, mode);
 475 }
 476
 477 void mpsse_clock_data(struct mpsse_ctx *ctx, const uint8_t *out, unsigned out_offset, uint8_t *in,
 478         unsigned in_offset, unsigned length, uint8_t mode)
 479 {
 480         /* TODO: Fix MSB first modes */
 481         LOG_DEBUG_IO("%s%s %d bits", in ? "in" : "", out ? "out" : "", length);
 482
 483         if (ctx->retval != ERROR_OK) {
 484                 LOG_DEBUG_IO("Ignoring command due to previous error");
 485                 return;
 486         }
 487
 488         /* TODO: On H chips, use command 0x8E/0x8F if in and out are both 0 */
 489         if (out || (!out && !in))
 490                 mode |= 0x10;
 491         if (in)
 492                 mode |= 0x20;
 493
 494         while (length > 0) {
 495                 /* Guarantee buffer space enough for a minimum size transfer */
 496                 if (buffer_write_space(ctx) + (length < 8) < (out || (!out && !in) ? 4 : 3)
 497                                 || (in && buffer_read_space(ctx) < 1))
 498                         ctx->retval = mpsse_flush(ctx);
 499
 500                 if (length < 8) {
 501                         /* Transfer remaining bits in bit mode */
 502                         buffer_write_byte(ctx, 0x02 | mode);
 503                         buffer_write_byte(ctx, length - 1);
 504                         if (out)
 505                                 out_offset += buffer_write(ctx, out, out_offset, length);
 506                         if (in)
 507                                 in_offset += buffer_add_read(ctx, in, in_offset, length, 8 - length);
 508                         if (!out && !in)
 509                                 buffer_write_byte(ctx, 0x00);
 510                         length = 0;
 511                 } else {
 512                         /* Byte transfer */
 513                         unsigned this_bytes = length / 8;
 514                         /* MPSSE command limit */
 515                         if (this_bytes > 65536)
 516                                 this_bytes = 65536;
 517                         /* Buffer space limit. We already made sure there's space for the minimum
 518                          * transfer. */
 519                         if ((out || (!out && !in)) && this_bytes + 3 > buffer_write_space(ctx))
 520                                 this_bytes = buffer_write_space(ctx) - 3;
 521                         if (in && this_bytes > buffer_read_space(ctx))
 522                                 this_bytes = buffer_read_space(ctx);
 523
 524                         if (this_bytes > 0) {
 525                                 buffer_write_byte(ctx, mode);
 526                                 buffer_write_byte(ctx, (this_bytes - 1) & 0xff);
 527                                 buffer_write_byte(ctx, (this_bytes - 1) >> 8);
 528                                 if (out)
 529                                         out_offset += buffer_write(ctx,
 530                                                         out,
 531                                                         out_offset,
 532                                                         this_bytes * 8);
 533                                 if (in)
 534                                         in_offset += buffer_add_read(ctx,
 535                                                         in,
 536                                                         in_offset,
 537                                                         this_bytes * 8,
 538                                                         0);
 539                                 if (!out && !in)
 540                                         for (unsigned n = 0; n < this_bytes; n++)
 541                                                 buffer_write_byte(ctx, 0x00);
 542                                 length -= this_bytes * 8;
 543                         }
 544                 }
 545         }
 546 }
 547
 548 void mpsse_clock_tms_cs_out(struct mpsse_ctx *ctx, const uint8_t *out, unsigned out_offset,
 549         unsigned length, bool tdi, uint8_t mode)
 550 {
 551         mpsse_clock_tms_cs(ctx, out, out_offset, NULL, 0, length, tdi, mode);
 552 }
 553
 554 void mpsse_clock_tms_cs(struct mpsse_ctx *ctx, const uint8_t *out, unsigned out_offset, uint8_t *in,
 555         unsigned in_offset, unsigned length, bool tdi, uint8_t mode)
 556 {
 557         LOG_DEBUG_IO("%sout %d bits, tdi=%d", in ? "in" : "", length, tdi);
 558         assert(out);
 559
 560         if (ctx->retval != ERROR_OK) {
 561                 LOG_DEBUG_IO("Ignoring command due to previous error");
 562                 return;
 563         }
 564
 565         mode |= 0x42;
 566         if (in)
 567                 mode |= 0x20;
 568
 569         while (length > 0) {
 570                 /* Guarantee buffer space enough for a minimum size transfer */
 571                 if (buffer_write_space(ctx) < 3 || (in && buffer_read_space(ctx) < 1))
 572                         ctx->retval = mpsse_flush(ctx);
 573
 574                 /* Byte transfer */
 575                 unsigned this_bits = length;
 576                 /* MPSSE command limit */
 577                 /* NOTE: there's a report of an FT2232 bug in this area, where shifting
 578                  * exactly 7 bits can make problems with TMS signaling for the last
 579                  * clock cycle:
 580                  *
 581                  * http://developer.intra2net.com/mailarchive/html/libftdi/2009/msg00292.html
 582                  */
 583                 if (this_bits > 7)
 584                         this_bits = 7;
 585
 586                 if (this_bits > 0) {
 587                         buffer_write_byte(ctx, mode);
 588                         buffer_write_byte(ctx, this_bits - 1);
 589                         uint8_t data = 0;
 590                         /* TODO: Fix MSB first, if allowed in MPSSE */
 591                         bit_copy(&data, 0, out, out_offset, this_bits);
 592                         out_offset += this_bits;
 593                         buffer_write_byte(ctx, data | (tdi ? 0x80 : 0x00));
 594                         if (in)
 595                                 in_offset += buffer_add_read(ctx,
 596                                                 in,
 597                                                 in_offset,
 598                                                 this_bits,
 599                                                 8 - this_bits);
 600                         length -= this_bits;
 601                 }
 602         }
 603 }
 604
 605 void mpsse_set_data_bits_low_byte(struct mpsse_ctx *ctx, uint8_t data, uint8_t dir)
 606 {
 607         LOG_DEBUG_IO("-");
 608
 609         if (ctx->retval != ERROR_OK) {
 610                 LOG_DEBUG_IO("Ignoring command due to previous error");
 611                 return;
 612         }
 613
 614         if (buffer_write_space(ctx) < 3)
 615                 ctx->retval = mpsse_flush(ctx);
 616
 617         buffer_write_byte(ctx, 0x80);
 618         buffer_write_byte(ctx, data);
 619         buffer_write_byte(ctx, dir);
 620 }
 621
 622 void mpsse_set_data_bits_high_byte(struct mpsse_ctx *ctx, uint8_t data, uint8_t dir)
 623 {
 624         LOG_DEBUG_IO("-");
 625
 626         if (ctx->retval != ERROR_OK) {
 627                 LOG_DEBUG_IO("Ignoring command due to previous error");
 628                 return;
 629         }
 630
 631         if (buffer_write_space(ctx) < 3)
 632                 ctx->retval = mpsse_flush(ctx);
 633
 634         buffer_write_byte(ctx, 0x82);
 635         buffer_write_byte(ctx, data);
 636         buffer_write_byte(ctx, dir);
 637 }
 638
 639 void mpsse_read_data_bits_low_byte(struct mpsse_ctx *ctx, uint8_t *data)
 640 {
 641         LOG_DEBUG_IO("-");
 642
 643         if (ctx->retval != ERROR_OK) {
 644                 LOG_DEBUG_IO("Ignoring command due to previous error");
 645                 return;
 646         }
 647
 648         if (buffer_write_space(ctx) < 1 || buffer_read_space(ctx) < 1)
 649                 ctx->retval = mpsse_flush(ctx);
 650
 651         buffer_write_byte(ctx, 0x81);
 652         buffer_add_read(ctx, data, 0, 8, 0);
 653 }
 654
 655 void mpsse_read_data_bits_high_byte(struct mpsse_ctx *ctx, uint8_t *data)
 656 {
 657         LOG_DEBUG_IO("-");
 658
 659         if (ctx->retval != ERROR_OK) {
 660                 LOG_DEBUG_IO("Ignoring command due to previous error");
 661                 return;
 662         }
 663
 664         if (buffer_write_space(ctx) < 1 || buffer_read_space(ctx) < 1)
 665                 ctx->retval = mpsse_flush(ctx);
 666
 667         buffer_write_byte(ctx, 0x83);
 668         buffer_add_read(ctx, data, 0, 8, 0);
 669 }
 670
 671 static void single_byte_boolean_helper(struct mpsse_ctx *ctx, bool var, uint8_t val_if_true,
 672         uint8_t val_if_false)
 673 {
 674         if (ctx->retval != ERROR_OK) {
 675                 LOG_DEBUG_IO("Ignoring command due to previous error");
 676                 return;
 677         }
 678
 679         if (buffer_write_space(ctx) < 1)
 680                 ctx->retval = mpsse_flush(ctx);
 681
 682         buffer_write_byte(ctx, var ? val_if_true : val_if_false);
 683 }
 684
 685 void mpsse_loopback_config(struct mpsse_ctx *ctx, bool enable)
 686 {
 687         LOG_DEBUG("%s", enable ? "on" : "off");
 688         single_byte_boolean_helper(ctx, enable, 0x84, 0x85);
 689 }
 690
 691 void mpsse_set_divisor(struct mpsse_ctx *ctx, uint16_t divisor)
 692 {
 693         LOG_DEBUG("%d", divisor);
 694
 695         if (ctx->retval != ERROR_OK) {
 696                 LOG_DEBUG_IO("Ignoring command due to previous error");
 697                 return;
 698         }
 699
 700         if (buffer_write_space(ctx) < 3)
 701                 ctx->retval = mpsse_flush(ctx);
 702
 703         buffer_write_byte(ctx, 0x86);
 704         buffer_write_byte(ctx, divisor & 0xff);
 705         buffer_write_byte(ctx, divisor >> 8);
 706 }
 707
 708 int mpsse_divide_by_5_config(struct mpsse_ctx *ctx, bool enable)
 709 {
 710         if (!mpsse_is_high_speed(ctx))
 711                 return ERROR_FAIL;
 712
 713         LOG_DEBUG("%s", enable ? "on" : "off");
 714         single_byte_boolean_helper(ctx, enable, 0x8b, 0x8a);
 715
 716         return ERROR_OK;
 717 }
 718
 719 int mpsse_rtck_config(struct mpsse_ctx *ctx, bool enable)
 720 {
 721         if (!mpsse_is_high_speed(ctx))
 722                 return ERROR_FAIL;
 723
 724         LOG_DEBUG("%s", enable ? "on" : "off");
 725         single_byte_boolean_helper(ctx, enable, 0x96, 0x97);
 726
 727         return ERROR_OK;
 728 }
 729
 730 int mpsse_set_frequency(struct mpsse_ctx *ctx, int frequency)
 731 {
 732         LOG_DEBUG("target %d Hz", frequency);
 733         assert(frequency >= 0);
 734         int base_clock;
 735
 736         if (frequency == 0)
 737                 return mpsse_rtck_config(ctx, true);
 738
 739         mpsse_rtck_config(ctx, false); /* just try */
 740
 741         if (frequency > 60000000 / 2 / 65536 && mpsse_divide_by_5_config(ctx, false) == ERROR_OK) {
 742                 base_clock = 60000000;
 743         } else {
 744                 mpsse_divide_by_5_config(ctx, true); /* just try */
 745                 base_clock = 12000000;
 746         }
 747
 748         int divisor = (base_clock / 2 + frequency - 1) / frequency - 1;
 749         if (divisor > 65535)
 750                 divisor = 65535;
 751         assert(divisor >= 0);
 752
 753         mpsse_set_divisor(ctx, divisor);
 754
 755         frequency = base_clock / 2 / (1 + divisor);
 756         LOG_DEBUG("actually %d Hz", frequency);
 757
 758         return frequency;
 759 }
 760
 761 /* Context needed by the callbacks */
 762 struct transfer_result {
 763         struct mpsse_ctx *ctx;
 764         bool done;
 765         unsigned transferred;
 766 };
 767
 768 static LIBUSB_CALL void read_cb(struct libusb_transfer *transfer)
 769 {
 770         struct transfer_result *res = transfer->user_data;
 771         struct mpsse_ctx *ctx = res->ctx;
 772
 773         unsigned packet_size = ctx->max_packet_size;
 774
 775         DEBUG_PRINT_BUF(transfer->buffer, transfer->actual_length);
 776
 777         /* Strip the two status bytes sent at the beginning of each USB packet
 778          * while copying the chunk buffer to the read buffer */
 779         unsigned num_packets = DIV_ROUND_UP(transfer->actual_length, packet_size);
 780         unsigned chunk_remains = transfer->actual_length;
 781         for (unsigned i = 0; i < num_packets && chunk_remains > 2; i++) {
 782                 unsigned this_size = packet_size - 2;
 783                 if (this_size > chunk_remains - 2)
 784                         this_size = chunk_remains - 2;
 785                 if (this_size > ctx->read_count - res->transferred)
 786                         this_size = ctx->read_count - res->transferred;
 787                 memcpy(ctx->read_buffer + res->transferred,
 788                         ctx->read_chunk + packet_size * i + 2,
 789                         this_size);
 790                 res->transferred += this_size;
 791                 chunk_remains -= this_size + 2;
 792                 if (res->transferred == ctx->read_count) {
 793                         res->done = true;
 794                         break;
 795                 }
 796         }
 797
 798         LOG_DEBUG_IO("raw chunk %d, transferred %d of %d", transfer->actual_length, res->transferred,
 799                 ctx->read_count);
 800
 801         if (!res->done)
 802                 if (libusb_submit_transfer(transfer) != LIBUSB_SUCCESS)
 803                         res->done = true;
 804 }
 805
 806 static LIBUSB_CALL void write_cb(struct libusb_transfer *transfer)
 807 {
 808         struct transfer_result *res = transfer->user_data;
 809         struct mpsse_ctx *ctx = res->ctx;
 810
 811         res->transferred += transfer->actual_length;
 812
 813         LOG_DEBUG_IO("transferred %d of %d", res->transferred, ctx->write_count);
 814
 815         DEBUG_PRINT_BUF(transfer->buffer, transfer->actual_length);
 816
 817         if (res->transferred == ctx->write_count)
 818                 res->done = true;
 819         else {
 820                 transfer->length = ctx->write_count - res->transferred;
 821                 transfer->buffer = ctx->write_buffer + res->transferred;
 822                 if (libusb_submit_transfer(transfer) != LIBUSB_SUCCESS)
 823                         res->done = true;
 824         }
 825 }
 826
 827 int mpsse_flush(struct mpsse_ctx *ctx)
 828 {
 829         int retval = ctx->retval;
 830
 831         if (retval != ERROR_OK) {
 832                 LOG_DEBUG_IO("Ignoring flush due to previous error");
 833                 assert(ctx->write_count == 0 && ctx->read_count == 0);
 834                 ctx->retval = ERROR_OK;
 835                 return retval;
 836         }
 837
 838         LOG_DEBUG_IO("write %d%s, read %d", ctx->write_count, ctx->read_count ? "+1" : "",
 839                         ctx->read_count);
 840         assert(ctx->write_count > 0 || ctx->read_count == 0); /* No read data without write data */
 841
 842         if (ctx->write_count == 0)
 843                 return retval;
 844
 845         struct libusb_transfer *read_transfer = NULL;
 846         struct transfer_result read_result = { .ctx = ctx, .done = true };
 847         if (ctx->read_count) {
 848                 buffer_write_byte(ctx, 0x87); /* SEND_IMMEDIATE */
 849                 read_result.done = false;
 850                 /* delay read transaction to ensure the FTDI chip can support us with data
 851                    immediately after processing the MPSSE commands in the write transaction */
 852         }
 853
 854         struct transfer_result write_result = { .ctx = ctx, .done = false };
 855         struct libusb_transfer *write_transfer = libusb_alloc_transfer(0);
 856         libusb_fill_bulk_transfer(write_transfer, ctx->usb_dev, ctx->out_ep, ctx->write_buffer,
 857                 ctx->write_count, write_cb, &write_result, ctx->usb_write_timeout);
 858         retval = libusb_submit_transfer(write_transfer);
 859         if (retval != LIBUSB_SUCCESS)
 860                 goto error_check;
 861
 862         if (ctx->read_count) {
 863                 read_transfer = libusb_alloc_transfer(0);
 864                 libusb_fill_bulk_transfer(read_transfer, ctx->usb_dev, ctx->in_ep, ctx->read_chunk,
 865                         ctx->read_chunk_size, read_cb, &read_result,
 866                         ctx->usb_read_timeout);
 867                 retval = libusb_submit_transfer(read_transfer);
 868                 if (retval != LIBUSB_SUCCESS)
 869                         goto error_check;
 870         }
 871
 872         /* Polling loop, more or less taken from libftdi */
 873         int64_t start = timeval_ms();
 874         int64_t warn_after = 2000;
 875         while (!write_result.done || !read_result.done) {
 876                 struct timeval timeout_usb;
 877
 878                 timeout_usb.tv_sec = 1;
 879                 timeout_usb.tv_usec = 0;
 880
 881                 retval = libusb_handle_events_timeout_completed(ctx->usb_ctx, &timeout_usb, NULL);
 882                 keep_alive();
 883
 884                 int64_t now = timeval_ms();
 885                 if (now - start > warn_after) {
 886                         LOG_WARNING("Haven't made progress in mpsse_flush() for %" PRId64
 887                                         "ms.", now - start);
 888                         warn_after *= 2;
 889                 }
 890
 891                 if (retval == LIBUSB_ERROR_INTERRUPTED)
 892                         continue;
 893
 894                 if (retval != LIBUSB_SUCCESS) {
 895                         libusb_cancel_transfer(write_transfer);
 896                         if (read_transfer)
 897                                 libusb_cancel_transfer(read_transfer);
 898                 }
 899         }
 900
 901 error_check:
 902         if (retval != LIBUSB_SUCCESS) {
 903                 LOG_ERROR("libusb_handle_events() failed with %s", libusb_error_name(retval));
 904                 retval = ERROR_FAIL;
 905         } else if (write_result.transferred < ctx->write_count) {
 906                 LOG_ERROR("ftdi device did not accept all data: %d, tried %d",
 907                         write_result.transferred,
 908                         ctx->write_count);
 909                 retval = ERROR_FAIL;
 910         } else if (read_result.transferred < ctx->read_count) {
 911                 LOG_ERROR("ftdi device did not return all data: %d, expected %d",
 912                         read_result.transferred,
 913                         ctx->read_count);
 914                 retval = ERROR_FAIL;
 915         } else if (ctx->read_count) {
 916                 ctx->write_count = 0;
 917                 ctx->read_count = 0;
 918                 bit_copy_execute(&ctx->read_queue);
 919                 retval = ERROR_OK;
 920         } else {
 921                 ctx->write_count = 0;
 922                 bit_copy_discard(&ctx->read_queue);
 923                 retval = ERROR_OK;
 924         }
 925
 926         if (retval != ERROR_OK)
 927                 mpsse_purge(ctx);
 928
 929         libusb_free_transfer(write_transfer);
 930         if (read_transfer)
 931                 libusb_free_transfer(read_transfer);
 932
 933         return retval;
 934 }