xusb.c

/* xusb: Generic USB test program
 * Copyright © 2009-2012 Pete Batard <pete@akeo.ie>
 * Contributions to Mass Storage by Alan Stern.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <libusb-1.0/libusb.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "config.h"

/* begin of code from xusb.c */
#define ERR_EXIT(errcode)                                                      \
  do {                                                                         \
    perr("   %s\n", libusb_strerror((enum libusb_error)errcode));              \
    return -1;                                                                 \
  } while (0)
#define CALL_CHECK(fcall)                                                      \
  do {                                                                         \
    int _r = fcall;                                                            \
    if (_r < 0)                                                                \
      ERR_EXIT(_r);                                                            \
  } while (0)
#define CALL_CHECK_CLOSE(fcall, hdl)                                           \
  do {                                                                         \
    int _r = fcall;                                                            \
    if (_r < 0) {                                                              \
      libusb_close(hdl);                                                       \
      ERR_EXIT(_r);                                                            \
    }                                                                          \
  } while (0)

// Section 5.1: Command Block Wrapper (CBW)
struct command_block_wrapper {
  uint8_t dCBWSignature[4];
  uint32_t dCBWTag;
  uint32_t dCBWDataTransferLength;
  uint8_t bmCBWFlags;
  uint8_t bCBWLUN;
  uint8_t bCBWCBLength;
  uint8_t CBWCB[16];
};

// Section 5.2: Command Status Wrapper (CSW)
struct command_status_wrapper {
  uint8_t dCSWSignature[4];
  uint32_t dCSWTag;
  uint32_t dCSWDataResidue;
  uint8_t bCSWStatus;
};

const uint8_t cdb_length[256] = {
    // 0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F
    06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, //  0
    06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, 06, //  1
    10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, //  2
    10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, //  3
    10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, //  4
    10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, //  5
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, //  6
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, //  7
    16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, //  8
    16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, //  9
    12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, //  A
    12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, //  B
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, //  C
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, //  D
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, //  E
    /* Added length parameters for vendor commands.
     *                                     —— —— —— */
    00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 16, 16, 16, //  F
};

static void perr(char const *format, ...) {
  va_list args;

  va_start(args, format);
  vfprintf(stderr, format, args);
  va_end(args);
}

int send_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint,
                              uint8_t lun, uint8_t *cdb, uint8_t direction,
                              int data_length, uint32_t *ret_tag) {
  static uint32_t tag = 1;
  uint8_t cdb_len;
  int i, r, size;
  struct command_block_wrapper cbw;

  if (cdb == NULL) {
    return -1;
  }

  if (endpoint & LIBUSB_ENDPOINT_IN) {
    perr("send_mass_storage_command: cannot send command on IN endpoint\n");
    return -1;
  }

  cdb_len = cdb_length[cdb[0]];
  if ((cdb_len == 0) || (cdb_len > sizeof(cbw.CBWCB))) {
    perr("send_mass_storage_command: don't know how to handle this command "
         "(%02X, length %d)\n",
         cdb[0], cdb_len);
    return -1;
  }

  memset(&cbw, 0, sizeof(cbw));
  cbw.dCBWSignature[0] = 'U';
  cbw.dCBWSignature[1] = 'S';
  cbw.dCBWSignature[2] = 'B';
  cbw.dCBWSignature[3] = 'C';
  *ret_tag = tag;
  cbw.dCBWTag = tag++;
  cbw.dCBWDataTransferLength = data_length;
  cbw.bmCBWFlags = direction;
  cbw.bCBWLUN = lun;
  // Subclass is 1 or 6 => cdb_len
  cbw.bCBWCBLength = cdb_len;
  memcpy(cbw.CBWCB, cdb, cdb_len);

  i = 0;
  do {
    // The transfer length must always be exactly 31 bytes.
    r = libusb_bulk_transfer(handle, endpoint, (unsigned char *)&cbw, 31, &size,
                             1000);
    if (r == LIBUSB_ERROR_PIPE) {
      libusb_clear_halt(handle, endpoint);
    }
    i++;
  } while ((r == LIBUSB_ERROR_PIPE) && (i < RETRY_MAX));
  if (r != LIBUSB_SUCCESS) {
    perr("send_mass_storage_command: %s\n",
         libusb_strerror((enum libusb_error)r));
    return -1;
  }

  return 0;
}