138 lines
4.7 KiB
C
138 lines
4.7 KiB
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 <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <stdarg.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <libusb-1.0/libusb.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;
|
|
}
|