From 98386e6e367caed339f5a730936b1186ffdf4257 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Thu, 1 May 2008 23:51:35 +0200 Subject: [PATCH] Added initial usbstorage code. There are still some thing missing like error handling but this should be working as long as nothing goes seriously wrong. Please note that there are probably still some bugs that need to be fixed. --- usbstorage.c | 474 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ usbstorage.h | 41 ++++++ 2 files changed, 515 insertions(+) create mode 100644 usbstorage.c create mode 100644 usbstorage.h diff --git a/usbstorage.c b/usbstorage.c new file mode 100644 index 0000000..b80a5a0 --- /dev/null +++ b/usbstorage.c @@ -0,0 +1,474 @@ +/*------------------------------------------------------------- + +usbstorage.c -- Bulk-only USB mass storage support + +Copyright (C) 2008 +Sven Peter (svpe) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you +must not claim that you wrote the original software. If you use +this software in a product, an acknowledgment in the product +documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +-------------------------------------------------------------*/ +#include +#include +#include + +#include "usbstorage.h" + +#define HEAP_SIZE 4096 +#define TAG_START 0x0BADC0DE + +#define CBW_SIZE 31 +#define CBW_SIGNATURE 0x43425355 +#define CBW_IN (1 << 7) +#define CBW_OUT 0 + +#define CSW_SIZE 13 +#define CSW_SIGNATURE 0x53425355 + +#define SCSI_TEST_UNIT_READY 0x00 +#define SCSI_REQUEST_SENSE 0x03 +#define SCSI_READ_CAPACITY 0x25 +#define SCSI_READ_10 0x28 +#define SCSI_WRITE_10 0x2A + +#define SCSI_SENSE_REPLY_SIZE 18 + +#ifdef DEBUG +#include +#define TRACE printf(" %d@%s\n", __LINE__, __FUNCTION__); +#else +#define TRACE +#endif + +static s32 hId; + +s32 USBStorage_Initialize() +{ + hId = iosCreateHeap(HEAP_SIZE); + + if(hId < 0) + return IPC_ENOMEM; + + return IPC_OK; + +} + +s32 USBStorage_Deinitialize() +{ + return iosDestroyHeap(hId); +} + +static inline void __write32(u8 *p, u32 v) +{ + p[0] = v; + p[1] = v >> 8; + p[2] = v >> 16; + p[3] = v >> 24; +} + +static inline u32 __read32(u8 *p) +{ + return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24); +} + +static s32 __send_cbw(usbstorage_handle *dev, u32 len, u8 flags, const u8 *cb, u8 cbLen) +{ + u8 *cbw = NULL; + s32 retval; + + if(cbLen == 0 || cbLen > 16) + return IPC_EINVAL; + + cbw = iosAlloc(hId, CBW_SIZE); + if(cbw == NULL) + { + TRACE + retval = IPC_ENOHEAP; + goto free_and_return; + } + + memset(cbw, 0, CBW_SIZE); + + __write32(cbw, CBW_SIGNATURE); + __write32(cbw + 4, dev->tag); + __write32(cbw + 8, len); + cbw[12] = flags; + cbw[13] = 0; /* TODO: LUN */ + cbw[14] = cbLen; + + memcpy(cbw + 15, cb, cbLen); + + LWP_MutexLock(dev->usb_fd); + retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, CBW_SIZE, (void *)cbw); + + if(retval == CBW_SIZE) + retval = 0; + else if(retval > CBW_SIZE) + retval = IPC_EINVAL; + + free_and_return: + if(cbw != NULL) + iosFree(hId, cbw); + return retval; +} + +static s32 __read_csw(usbstorage_handle *dev, u8 *status, u32 *dataResidue) +{ + u8 *csw = NULL; + s32 retval; + u32 signature, tag, _dataResidue, _status; + + csw = iosAlloc(hId, CSW_SIZE); + if(csw == NULL) + { + TRACE + retval = IPC_ENOHEAP; + goto free_and_return; + } + + memset(csw, 0, CSW_SIZE); + + retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, CSW_SIZE, csw); + if(retval != CSW_SIZE) + { + TRACE + if(retval > 0) + retval = IPC_EINVAL; + goto free_and_return; + } + + signature = __read32(csw); + tag = __read32(csw + 4); + _dataResidue = __read32(csw + 8); + _status = csw[12]; + + if(signature != CSW_SIGNATURE) + { + TRACE + retval = IPC_EINVAL; + goto free_and_return; + } + + if(dataResidue != NULL) + *dataResidue = _dataResidue; + if(status != NULL) + *status = _status; + + if(tag != dev->tag) + { + TRACE + retval = IPC_EINVAL; + goto free_and_return; + } + + dev->tag++; + + LWP_MutexUnlock(dev->lock); + + free_and_return: + if(csw != NULL) + iosFree(hId, csw); + return retval; +} + +static s32 __read_cycle(usbstorage_handle *dev, u8 *buffer, u32 len, u8 *cb, u8 cbLen) +{ + s32 retval; + u8 *bfr; + + u8 status; + u32 dataResidue; + + bfr = iosAlloc(hId, dev->ep_in_size); + if(bfr == NULL) + { + TRACE + retval = IPC_ENOHEAP; + goto free_and_error; + } + + retval = __send_cbw(dev, len, CBW_IN, cb, cbLen); + if(retval < 0) + goto free_and_error; + + while(len > 0) + { + TRACE + retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, dev->ep_in_size, bfr); + if(retval < 0) + { + TRACE + goto free_and_error; + } + + memcpy(buffer, bfr, retval); + len -= retval; + buffer += retval; + + if(retval != dev->ep_in_size) + break; + } + + retval = __read_csw(dev, &status, &dataResidue); + if(retval < 0) + { + TRACE + goto free_and_error; + } + + if(status || dataResidue) + { + TRACE + // TODO error handling + retval = -1; + goto free_and_error; + } + + free_and_error: + TRACE + if(bfr != NULL) + iosFree(hId, bfr); + return retval; +} + +static s32 __write_cycle(usbstorage_handle *dev, const u8 *buffer, u32 len, u8 *cb, u8 cbLen) +{ + s32 retval; + u8 *bfr; + u32 thisLen; + + u8 status; + u32 dataResidue; + + bfr = iosAlloc(hId, dev->ep_out_size); + if(bfr == NULL) + { + TRACE + retval = IPC_ENOHEAP; + goto free_and_error; + } + + retval = __send_cbw(dev, len, CBW_OUT, cb, cbLen); + if(retval < 0) + goto free_and_error; + + while(len > 0) + { + TRACE + thisLen = len > dev->ep_out_size ? dev->ep_out_size : len; + memset(bfr, 0, dev->ep_out_size); + memcpy(bfr, buffer, thisLen); + retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, thisLen, bfr); + if(retval < 0) + { + TRACE + goto free_and_error; + } + + len -= retval; + buffer += retval; + + if(retval != thisLen && len > 0) + { + retval = -1; + goto free_and_error; + } + } + + retval = __read_csw(dev, &status, &dataResidue); + if(retval < 0) + { + TRACE + goto free_and_error; + } + + if(status || dataResidue) + { + TRACE + // TODO error handling + retval = -1; + goto free_and_error; + } + + free_and_error: + TRACE + if(buffer != NULL) + iosFree(hId, bfr); + return retval; +} + +usbstorage_handle *USBStorage_Open(const char *bus, u16 vid, u16 pid) +{ + usbstorage_handle *dev = NULL; + u8 cmd[16]; + u8 *sense = NULL; + u8 status = 0; + s32 retval = -1; + + dev = calloc(1, sizeof(*dev)); + if(dev == NULL) + { + retval = IPC_ENOMEM; + goto free_and_return; + } + + dev->tag = TAG_START; + if(LWP_MutexInit(&dev->lock, true) < 0) + goto free_and_return; + LWP_MutexLock(dev->lock); + + retval = USB_OpenDevice(bus, vid, pid, &dev->usb_fd); + if(retval < 0) + goto free_and_return; + + sense = iosAlloc(hId, SCSI_SENSE_REPLY_SIZE); + if(sense == NULL) + goto free_and_return; + + // TODO: parse descriptors here + dev->ep_in = 0x81; + dev->ep_in_size = 64; + dev->ep_out = 0x02; + dev->ep_out_size = 64; + + memset(cmd, 0, sizeof(cmd)); + cmd[0] = SCSI_TEST_UNIT_READY; + + TRACE + retval = __send_cbw(dev, 0, CBW_IN, cmd, 1); + + if(retval < 0) + { + /* fatal error */ + TRACE + retval = -1; + goto free_and_return; + } + + /* some devices needs this to be fully working... */ + retval = __read_csw(dev, &status, NULL); + if(status != 0) + { + TRACE + cmd[0] = SCSI_REQUEST_SENSE; + cmd[4] = SCSI_SENSE_REPLY_SIZE; + cmd[5] = 0; + retval = __send_cbw(dev, 0, CBW_IN, cmd, 6); + if(retval < 0) + { + /* fatal error */ + TRACE + retval = -1; + goto free_and_return; + } + + memset(sense, 0, SCSI_SENSE_REPLY_SIZE); + retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, SCSI_SENSE_REPLY_SIZE, sense); +#if 0 + if(retval < SCSI_SENSE_REPLY_SIZE) +#else + if(retval < 0) /* this retruns 0 on one of my sticks.. :/ */ +#endif + { + /* fatal error again */ + TRACE + retval = -1; + goto free_and_return; + } + + retval = __read_csw(dev, NULL, NULL); + if(retval < 0) + { + /* fatal error again */ + TRACE + retval = -1; + goto free_and_return; + } + } + + memset(cmd, 0, sizeof(cmd)); + memset(sense, 0, 8); + cmd[0] = SCSI_READ_CAPACITY; + retval = __read_cycle(dev, sense, 8, cmd, 1); + if(retval < 0) + { + TRACE + goto free_and_return; + } + memcpy(&dev->n_sectors, sense, 4); + memcpy(&dev->sector_size, sense + 4, 4); + + free_and_return: + LWP_MutexUnlock(dev->lock); + if(sense != NULL) + iosFree(hId, sense); + if(retval < 0) + { + TRACE + LWP_MutexDestroy(dev->lock); + free(dev); + return NULL; + } + return dev; +} + +s32 USBStorage_Close(usbstorage_handle *dev) +{ + USB_CloseDevice(&dev->usb_fd); + LWP_MutexDestroy(dev->lock); + memset(dev, 0, sizeof(*dev)); + return 0; +} + +s32 USBStorage_Read(usbstorage_handle *dev, u32 sector, u8 *buffer, u16 n_sectors) +{ + u8 cmd[] = { + SCSI_READ_10, + 0, + sector >> 24, + sector >> 16, + sector >> 8, + sector, + 0, + n_sectors >> 8, + n_sectors, + 0 + }; + return __read_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd)); +} + +// not tested yet +s32 USBStorage_Write(usbstorage_handle *dev, u32 sector, const u8 *buffer, u16 n_sectors) +{ + u8 cmd[] = { + SCSI_WRITE_10, + sector >> 24, + sector >> 16, + sector >> 8, + sector, + 0, + n_sectors >> 8, + n_sectors, + 0 + }; + + return __write_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd)); + +} diff --git a/usbstorage.h b/usbstorage.h new file mode 100644 index 0000000..e580360 --- /dev/null +++ b/usbstorage.h @@ -0,0 +1,41 @@ +#ifndef __USBSTORAGE_H +#define __USBSTORAGE_H 1 + +#include + +typedef struct +{ + u8 configuration; + u32 interface; + u32 altInterface; + + u8 ep_in; + u8 ep_out; + + u32 ep_in_size; + u32 ep_out_size; + + u32 sector_size; + u32 n_sectors; + + s32 usb_fd; + + mutex_t lock; + + u32 tag; +} usbstorage_handle; + +s32 USB_GetConfiguration(s32 fd, int index, usb_configurationdesc *ucd); + +s32 USBStorage_Initialize(); +s32 USBStorage_Deinitialize(); + +usbstorage_handle *USBStorage_Open(const char *bus, u16 vid, u16 pid); +s32 USBStorage_Close(usbstorage_handle *dev); + +s32 USBStorage_ReadCapacity(usbstorage_handle *dev, u32 *sector_size, u32 *n_sectors); + +s32 USBStorage_Read(usbstorage_handle *dev, u32 sector, u8 *buffer, u16 n_sectors); +s32 USBStorage_Write(usbstorage_handle *dev, u32 sector, const u8 *buffer, u16 n_sectors); + +#endif -- 2.11.4.GIT