diff --git a/README.md b/README.md index 9e7a877..cc2a973 100644 --- a/README.md +++ b/README.md @@ -320,3 +320,6 @@ Synchrize time `0xff`: # 3 seconds later: 0000 ff 00 07 e5 08 0a 0b 24 06 00 00 00 00 00 00 00 .......$........ ``` + +**Current challange** +After requesting the `sense` and Direct Access the correct data is not sent. diff --git a/main.c b/main.c index 3b921bd..7d70574 100644 --- a/main.c +++ b/main.c @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -8,8 +9,116 @@ #include "config.h" +// 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; +}; + +static 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 + 00,00,00,00,00,00,00,00,00,00,00,00,00,00,00,00, // F +}; + + +#define REQUEST_SENSE_LENGTH 0x12 + + static struct libusb_device_handle *devh = NULL; +static void perr(char const *format, ...) +{ + va_list args; + + va_start (args, format); + vfprintf(stderr, format, args); + va_end(args); +} + +static 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