static s32 __send_cbw(usbstorage_handle *dev, u8 lun, u32 len, u8 flags, const u8 *cb, u8 cbLen)
{
s32 retval = USBSTORAGE_OK;
if(cbLen == 0 || cbLen > 16)
return IPC_EINVAL;
memset(cbw_buffer, 0, CBW_SIZE);
__stwbrx(cbw_buffer, 0, CBW_SIGNATURE);
__stwbrx(cbw_buffer, 4, ++dev->tag);
__stwbrx(cbw_buffer, 8, len);
cbw_buffer[12] = flags;
cbw_buffer[13] = lun;
cbw_buffer[14] = (cbLen > 6 ? 10 : 6);
memcpy(cbw_buffer + 15, cb, cbLen);
if(dev->suspended == 1)
{
USB_OGC_ResumeDevice(dev->usb_fd);
dev->suspended = 0;
}
retval = __USB_BlkMsgTimeout(dev, dev->ep_out, CBW_SIZE, (void *)cbw_buffer, usbtimeout);
if(retval == CBW_SIZE) return USBSTORAGE_OK;
else if(retval > 0) return USBSTORAGE_ESHORTWRITE;
return retval;
}
static s32 __read_csw(usbstorage_handle *dev, u8 *status, u32 *dataResidue, u32 timeout)
{
s32 retval = USBSTORAGE_OK;
u32 signature, tag, _dataResidue, _status;
memset(cbw_buffer, 0, CSW_SIZE);
retval = __USB_BlkMsgTimeout(dev, dev->ep_in, CSW_SIZE, cbw_buffer, timeout);
if(retval > 0 && retval != CSW_SIZE) return USBSTORAGE_ESHORTREAD;
else if(retval < 0) return retval;
signature = __lwbrx(cbw_buffer, 0);
tag = __lwbrx(cbw_buffer, 4);
_dataResidue = __lwbrx(cbw_buffer, 8);
_status = cbw_buffer[12];
if(signature != CSW_SIGNATURE) return USBSTORAGE_ESIGNATURE;
if(dataResidue != NULL)
*dataResidue = _dataResidue;
if(status != NULL)
*status = _status;
if(tag != dev->tag) return USBSTORAGE_ETAG;
return USBSTORAGE_OK;
}
static s32 __read_csw(struct ehci_hcd * ehci, usbstorage_handle *dev, u8 *status, u32 *dataResidue) {
s32 retval = USBSTORAGE_OK;
u32 signature, tag, _dataResidue, _status;
memset(dev->buffer, 0xff, CSW_SIZE);
retval = __USB_BlkMsgTimeout(ehci, dev, dev->ep_in, CSW_SIZE, dev->buffer);
//print_hex_dump_bytes("csv resp:",DUMP_PREFIX_OFFSET,dev->buffer,CSW_SIZE);
if (retval >= 0 && retval != CSW_SIZE) return USBSTORAGE_ESHORTREAD;
else if (retval < 0) return retval;
signature = le32_to_cpu(((u32*) dev->buffer)[0]);
tag = le32_to_cpu(((u32*) dev->buffer)[1]);
_dataResidue = le32_to_cpu(((u32*) dev->buffer)[2]);
_status = dev->buffer[12];
//debug_printf("csv status: %d\n",_status);
if (signature != CSW_SIGNATURE) {
// BUG();
return USBSTORAGE_ESIGNATURE;
}
if (dataResidue != NULL)
*dataResidue = _dataResidue;
if (status != NULL)
*status = _status;
if (tag != dev->tag) return USBSTORAGE_ETAG;
dev->tag++;
return USBSTORAGE_OK;
}
static s32 __send_cbw(struct ehci_hcd * ehci, usbstorage_handle *dev, u8 lun, u32 len, u8 flags, const u8 *cb, u8 cbLen) {
s32 retval = USBSTORAGE_OK;
if (cbLen == 0 || cbLen > 16 || !dev->buffer)
return -EINVAL;
memset(dev->buffer, 0, CBW_SIZE);
((u32*) dev->buffer)[0] = cpu_to_le32(CBW_SIGNATURE);
((u32*) dev->buffer)[1] = cpu_to_le32(dev->tag);
((u32*) dev->buffer)[2] = cpu_to_le32(len);
dev->buffer[12] = flags;
dev->buffer[13] = lun;
// linux usb/storage/protocol.c seems to say only difference is padding
// and fixed cw size
if (dev->ata_protocol)
dev->buffer[14] = 12;
else
dev->buffer[14] = (cbLen > 6 ? 0x10 : 6);
//debug_printf("send cb of size %d\n",dev->buffer[14]);
memcpy(dev->buffer + 15, cb, cbLen);
//hexdump(dev->buffer,CBW_SIZE);
retval = __USB_BlkMsgTimeout(ehci, dev, dev->ep_out, CBW_SIZE, (void *) dev->buffer);
if (retval == CBW_SIZE) return USBSTORAGE_OK;
else if (retval >= 0) return USBSTORAGE_ESHORTWRITE;
return retval;
}
static s32 __send_cbw(usbstorage_handle *dev, u8 lun, u32 len, u8 flags, const u8 *cb, u8 cbLen)
{
s32 retval = USBSTORAGE_OK;
if(cbLen == 0 || cbLen > 16)
return -EINVAL;
memset(dev->buffer, 0, CBW_SIZE);
((u32*)dev->buffer)[0]=cpu_to_le32(CBW_SIGNATURE);
((u32*)dev->buffer)[1]=cpu_to_le32(dev->tag);
((u32*)dev->buffer)[2]=cpu_to_le32(len);
dev->buffer[12] = flags;
dev->buffer[13] = lun;
dev->buffer[14] = (cbLen > 6 ? 0x10 : 6);
memcpy(dev->buffer + 15, cb, cbLen);
retval = __USB_BlkMsgTimeout(dev, dev->ep_out, CBW_SIZE, (void *)dev->buffer);
if(retval == CBW_SIZE) return USBSTORAGE_OK;
else if(retval >= 0) return USBSTORAGE_ESHORTWRITE;
return retval;
}
static s32 __cycle(usbstorage_handle *dev, u8 lun, u8 *buffer, u32 len, u8 *cb, u8 cbLen, u8 write, u8 *_status, u32 *_dataResidue)
{
s32 retval = USBSTORAGE_OK;
u8 status=0;
u32 dataResidue = 0;
u16 max_size;
u8 ep = write ? dev->ep_out : dev->ep_in;
s8 retries = USBSTORAGE_CYCLE_RETRIES + 1;
if(usb2_mode)
max_size=MAX_TRANSFER_SIZE_V5;
else
max_size=MAX_TRANSFER_SIZE_V0;
LWP_MutexLock(dev->lock);
do
{
u8 *_buffer = buffer;
u32 _len = len;
retries--;
if(retval == USBSTORAGE_ETIMEDOUT)
break;
retval = __send_cbw(dev, lun, len, (write ? CBW_OUT:CBW_IN), cb, cbLen);
while(_len > 0 && retval >= 0)
{
u32 thisLen = _len > max_size ? max_size : _len;
if ((u32)_buffer&0x1F || !((u32)_buffer&0x10000000)) {
if (write) memcpy(dev->buffer, _buffer, thisLen);
retval = __USB_BlkMsgTimeout(dev, ep, thisLen, dev->buffer, usbtimeout);
if (!write && retval > 0)
memcpy(_buffer, dev->buffer, retval);
} else
retval = __USB_BlkMsgTimeout(dev, ep, thisLen, _buffer, usbtimeout);
if (retval == thisLen) {
_len -= retval;
_buffer += retval;
}
else if (retval != USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_EDATARESIDUE;
}
if (retval >= 0)
retval = __read_csw(dev, &status, &dataResidue, usbtimeout);
if (retval < 0) {
if (__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
}
} while (retval < 0 && retries > 0);
LWP_MutexUnlock(dev->lock);
if(_status != NULL)
*_status = status;
if(_dataResidue != NULL)
*_dataResidue = dataResidue;
return retval;
}
static s32 __cycle(struct ehci_hcd * ehci, usbstorage_handle *dev, u8 lun, u8 *buffer, u32 len, u8 *cb, u8 cbLen, u8 write, u8 *_status, u32 *_dataResidue) {
s32 retval = USBSTORAGE_OK;
u8 status = 0;
u32 dataResidue = 0;
u32 thisLen;
u8 *buffer2 = buffer;
u32 len2 = len;
s8 retries = USBSTORAGE_CYCLE_RETRIES + 1;
do {
if (retval == -ENODEV) {
unplug_device = 1;
return -ENODEV;
}
if (retval < 0)
retval = __usbstorage_reset(ehci, dev, retries < USBSTORAGE_CYCLE_RETRIES);
retries--;
if (retval < 0) continue; // nuevo
buffer = buffer2;
len = len2;
if (write) {
retval = __send_cbw(ehci, dev, lun, len, CBW_OUT, cb, cbLen);
if (retval < 0)
continue; //reset+retry
while (len > 0) {
thisLen = len;
retval = __USB_BlkMsgTimeout(ehci, dev, dev->ep_out, thisLen, buffer);
if (retval == -ENODEV || retval == -ETIMEDOUT) break;
if (retval < 0)
continue; //reset+retry
if (retval != thisLen && len > 0) {
retval = USBSTORAGE_EDATARESIDUE;
continue; //reset+retry
}
len -= retval;
buffer += retval;
}
if (retval < 0)
continue;
} else {
retval = __send_cbw(ehci, dev, lun, len, CBW_IN, cb, cbLen);
if (retval < 0)
continue; //reset+retry
while (len > 0) {
thisLen = len;
retval = __USB_BlkMsgTimeout(ehci, dev, dev->ep_in, thisLen, buffer);
if (retval == -ENODEV || retval == -ETIMEDOUT) break;
if (retval < 0)
continue; //reset+retry
//hexdump(buffer,retval);
len -= retval;
buffer += retval;
if (retval != thisLen) {
retval = -1;
continue; //reset+retry
}
}
if (retval < 0)
continue;
}
retval = __read_csw(ehci, dev, &status, &dataResidue);
if (retval < 0)
continue;
retval = USBSTORAGE_OK;
} while (retval < 0 && retries > 0);
// force unplug
if (retval < 0 && retries <= 0 && handshake_mode == 0) {
unplug_device = 1;
return -ENODEV;
}
if (retval < 0 && retval != USBSTORAGE_ETIMEDOUT) {
if (__usbstorage_reset(ehci, dev, 0) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
}
if (_status != NULL)
*_status = status;
if (_dataResidue != NULL)
*_dataResidue = dataResidue;
return retval;
}
static s32 __cycle(usbstorage_handle *dev, u8 lun, u8 *buffer, u32 len, u8 *cb, u8 cbLen, u8 write, u8 *_status, u32 *_dataResidue)
{
s32 retval = USBSTORAGE_OK;
u8 status = 0;
u32 dataResidue = 0;
u32 thisLen;
s8 retries = USBSTORAGE_CYCLE_RETRIES + 1;
do
{
retries--;
if(retval == USBSTORAGE_ETIMEDOUT)
break;
if(write)
{
retval = __send_cbw(dev, lun, len, CBW_OUT, cb, cbLen);
if(retval == USBSTORAGE_ETIMEDOUT)
break;
if(retval < 0)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
continue;
}
while(len > 0)
{
thisLen=len;
retval = __USB_BlkMsgTimeout(dev, dev->ep_out, thisLen, buffer);
if(retval == USBSTORAGE_ETIMEDOUT)
break;
if(retval < 0)
{
retval = USBSTORAGE_EDATARESIDUE;
break;
}
if(retval != thisLen && len > 0)
{
retval = USBSTORAGE_EDATARESIDUE;
break;
}
len -= retval;
buffer += retval;
}
if(retval < 0)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
continue;
}
}
else
{
retval = __send_cbw(dev, lun, len, CBW_IN, cb, cbLen);
if(retval == USBSTORAGE_ETIMEDOUT)
break;
if(retval < 0)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
continue;
}
while(len > 0)
{
thisLen=len;
retval = __USB_BlkMsgTimeout(dev, dev->ep_in, thisLen, buffer);
//print_hex_dump_bytes("usbs in:",DUMP_PREFIX_OFFSET,dev->buffer,36);
if(retval < 0)
break;
len -= retval;
buffer += retval;
if(retval != thisLen)
break;
}
if(retval < 0)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
continue;
}
}
retval = __read_csw(dev, &status, &dataResidue);
if(retval == USBSTORAGE_ETIMEDOUT)
break;
if(retval < 0)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
continue;
}
retval = USBSTORAGE_OK;
} while(retval < 0 && retries > 0);
if(retval < 0 && retval != USBSTORAGE_ETIMEDOUT)
{
if(__usbstorage_reset(dev) == USBSTORAGE_ETIMEDOUT)
retval = USBSTORAGE_ETIMEDOUT;
}
if(_status != NULL)
*_status = status;
if(_dataResidue != NULL)
*_dataResidue = dataResidue;
return retval;
}
