pi1541/uspi/lib/usbmassdevice.c

697 lines
18 KiB
C
Raw Normal View History

2018-05-20 04:53:34 +00:00
//
// usbmassdevice.c
//
// USPi - An USB driver for Raspberry Pi written in C
// Copyright (C) 2014 R. Stange <rsta2@o2online.de>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
#include <uspi/usbmassdevice.h>
#include <uspi/usbhostcontroller.h>
#include <uspi/devicenameservice.h>
#include <uspi/util.h>
#include <uspi/macros.h>
#include <uspi/assert.h>
#include <uspios.h>
// USB Mass Storage Bulk-Only Transport
// Command Block Wrapper
typedef struct TCBW
{
unsigned int dCWBSignature,
#define CBWSIGNATURE 0x43425355
dCWBTag,
dCBWDataTransferLength; // number of bytes
unsigned char bmCBWFlags,
#define CBWFLAGS_DATA_IN 0x80
bCBWLUN : 4,
#define CBWLUN 0
Reserved1 : 4,
bCBWCBLength : 5, // valid length of the CBWCB in bytes
Reserved2 : 3;
unsigned char CBWCB[16];
}
PACKED TCBW;
// Command Status Wrapper
typedef struct TCSW
{
unsigned int dCSWSignature,
#define CSWSIGNATURE 0x53425355
dCSWTag,
dCSWDataResidue; // difference in amount of data processed
unsigned char bCSWStatus;
#define CSWSTATUS_PASSED 0x00
#define CSWSTATUS_FAILED 0x01
#define CSWSTATUS_PHASE_ERROR 0x02
}
PACKED TCSW;
// SCSI Transparent Command Set
#define SCSI_CONTROL 0x00
typedef struct TSCSIInquiry
{
unsigned char OperationCode,
#define SCSI_OP_INQUIRY 0x12
LogicalUnitNumberEVPD,
PageCode,
Reserved,
AllocationLength,
Control;
}
PACKED TSCSIInquiry;
typedef struct TSCSIInquiryResponse
{
unsigned char PeripheralDeviceType : 5,
#define SCSI_PDT_DIRECT_ACCESS_BLOCK 0x00 // SBC-2 command set (or above)
#define SCSI_PDT_DIRECT_ACCESS_RBC 0x0E // RBC command set
PeripheralQualifier : 3, // 0: device is connected to this LUN
DeviceTypeModifier : 7,
RMB : 1, // 1: removable media
ANSIApprovedVersion : 3,
ECMAVersion : 3,
ISOVersion : 2,
Reserved1,
AdditionalLength,
Reserved2[3],
VendorIdentification[8],
ProductIdentification[16],
ProductRevisionLevel[4];
}
PACKED TSCSIInquiryResponse;
typedef struct TSCSITestUnitReady
{
unsigned char OperationCode;
#define SCSI_OP_TEST_UNIT_READY 0x00
unsigned int Reserved;
unsigned char Control;
}
PACKED TSCSITestUnitReady;
typedef struct TSCSIRequestSense
{
unsigned char OperationCode;
#define SCSI_REQUEST_SENSE 0x03
unsigned char DescriptorFormat : 1, // set to 0
Reserved1 : 7;
unsigned short Reserved2;
unsigned char AllocationLength;
unsigned char Control;
}
PACKED TSCSIRequestSense;
typedef struct TSCSIRequestSenseResponse7x
{
unsigned char ResponseCode : 7,
Valid : 1;
unsigned char Obsolete;
unsigned char SenseKey : 4,
Reserved : 1,
ILI : 1,
EOM : 1,
FileMark : 1;
unsigned int Information; // big endian
unsigned char AdditionalSenseLength;
unsigned int CommandSpecificInformation; // big endian
unsigned char AdditionalSenseCode;
unsigned char AdditionalSenseCodeQualifier;
unsigned char FieldReplaceableUnitCode;
unsigned char SenseKeySpecificHigh : 7,
SKSV : 1;
unsigned short SenseKeySpecificLow;
}
PACKED TSCSIRequestSenseResponse7x;
typedef struct TSCSIReadCapacity10
{
unsigned char OperationCode;
#define SCSI_OP_READ_CAPACITY10 0x25
unsigned char Obsolete : 1,
Reserved1 : 7;
unsigned int LogicalBlockAddress; // set to 0
unsigned short Reserved2;
unsigned char PartialMediumIndicator : 1, // set to 0
Reserved3 : 7;
unsigned char Control;
}
PACKED TSCSIReadCapacity10;
typedef struct TSCSIReadCapacityResponse
{
unsigned int ReturnedLogicalBlockAddress; // big endian
unsigned int BlockLengthInBytes; // big endian
}
PACKED TSCSIReadCapacityResponse;
typedef struct TSCSIRead10
{
unsigned char OperationCode,
#define SCSI_OP_READ 0x28
Reserved1;
unsigned int LogicalBlockAddress; // big endian
unsigned char Reserved2;
unsigned short TransferLength; // block count, big endian
unsigned char Control;
}
PACKED TSCSIRead10;
typedef struct TSCSIWrite10
{
unsigned char OperationCode,
#define SCSI_OP_WRITE 0x2A
Flags;
#define SCSI_WRITE_FUA 0x08
unsigned int LogicalBlockAddress; // big endian
unsigned char Reserved;
unsigned short TransferLength; // block count, big endian
unsigned char Control;
}
PACKED TSCSIWrite10;
static unsigned s_nDeviceNumber = 1;
static const char FromUmsd[] = "umsd";
int USBBulkOnlyMassStorageDeviceTryRead (TUSBBulkOnlyMassStorageDevice *pThis, void *pBuffer, unsigned nCount);
int USBBulkOnlyMassStorageDeviceTryWrite (TUSBBulkOnlyMassStorageDevice *pThis, const void *pBuffer, unsigned nCount);
int USBBulkOnlyMassStorageDeviceCommand (TUSBBulkOnlyMassStorageDevice *pThis,
void *pCmdBlk, unsigned nCmdBlkLen,
void *pBuffer, unsigned nBufLen, boolean bIn);
int USBBulkOnlyMassStorageDeviceReset (TUSBBulkOnlyMassStorageDevice *pThis);
void USBBulkOnlyMassStorageDevice (TUSBBulkOnlyMassStorageDevice *pThis, TUSBDevice *pDevice)
{
assert (pThis != 0);
USBDeviceCopy (&pThis->m_USBDevice, pDevice);
pThis->m_USBDevice.Configure = USBBulkOnlyMassStorageDeviceConfigure;
pThis->m_pEndpointIn = 0;
pThis->m_pEndpointOut = 0;
pThis->m_nCWBTag = 0;
pThis->m_nBlockCount = 0;
pThis->m_ullOffset = 0;
}
void _USBBulkOnlyMassStorageDevice (TUSBBulkOnlyMassStorageDevice *pThis)
{
assert (pThis != 0);
if (pThis->m_pEndpointOut != 0)
{
_USBEndpoint (pThis->m_pEndpointOut);
free (pThis->m_pEndpointOut);
pThis->m_pEndpointOut = 0;
}
if (pThis->m_pEndpointIn != 0)
{
_USBEndpoint (pThis->m_pEndpointIn);
free (pThis->m_pEndpointIn);
pThis->m_pEndpointIn = 0;
}
_USBDevice (&pThis->m_USBDevice);
}
boolean USBBulkOnlyMassStorageDeviceConfigure (TUSBDevice *pUSBDevice)
{
TUSBBulkOnlyMassStorageDevice *pThis = (TUSBBulkOnlyMassStorageDevice *) pUSBDevice;
assert (pThis != 0);
TUSBConfigurationDescriptor *pConfDesc =
(TUSBConfigurationDescriptor *) USBDeviceGetDescriptor (&pThis->m_USBDevice, DESCRIPTOR_CONFIGURATION);
if ( pConfDesc == 0
|| pConfDesc->bNumInterfaces < 1)
{
USBDeviceConfigurationError (&pThis->m_USBDevice, FromUmsd);
return FALSE;
}
TUSBInterfaceDescriptor *pInterfaceDesc =
(TUSBInterfaceDescriptor *) USBDeviceGetDescriptor (&pThis->m_USBDevice, DESCRIPTOR_INTERFACE);
if ( pInterfaceDesc == 0
|| pInterfaceDesc->bInterfaceNumber != 0x00
|| pInterfaceDesc->bAlternateSetting != 0x00
|| pInterfaceDesc->bNumEndpoints < 2
|| pInterfaceDesc->bInterfaceClass != 0x08 // Mass Storage Class
|| pInterfaceDesc->bInterfaceSubClass != 0x06 // SCSI Transparent Command Set
|| pInterfaceDesc->bInterfaceProtocol != 0x50) // Bulk-Only Transport
{
USBDeviceConfigurationError (&pThis->m_USBDevice, FromUmsd);
return FALSE;
}
const TUSBEndpointDescriptor *pEndpointDesc;
while ((pEndpointDesc = (TUSBEndpointDescriptor *) USBDeviceGetDescriptor (&pThis->m_USBDevice, DESCRIPTOR_ENDPOINT)) != 0)
{
if ((pEndpointDesc->bmAttributes & 0x3F) == 0x02) // Bulk
{
if ((pEndpointDesc->bEndpointAddress & 0x80) == 0x80) // Input
{
if (pThis->m_pEndpointIn != 0)
{
USBDeviceConfigurationError (&pThis->m_USBDevice, FromUmsd);
return FALSE;
}
pThis->m_pEndpointIn = (TUSBEndpoint *) malloc (sizeof (TUSBEndpoint));
assert (pThis->m_pEndpointIn != 0);
USBEndpoint2 (pThis->m_pEndpointIn, &pThis->m_USBDevice, pEndpointDesc);
}
else // Output
{
if (pThis->m_pEndpointOut != 0)
{
USBDeviceConfigurationError (&pThis->m_USBDevice, FromUmsd);
return FALSE;
}
pThis->m_pEndpointOut = (TUSBEndpoint *) malloc (sizeof (TUSBEndpoint));
assert (pThis->m_pEndpointOut != 0);
USBEndpoint2 (pThis->m_pEndpointOut, &pThis->m_USBDevice, pEndpointDesc);
}
}
}
if ( pThis->m_pEndpointIn == 0
|| pThis->m_pEndpointOut == 0)
{
USBDeviceConfigurationError (&pThis->m_USBDevice, FromUmsd);
return FALSE;
}
if (!USBDeviceConfigure (&pThis->m_USBDevice))
{
LogWrite (FromUmsd, LOG_ERROR, "Cannot set configuration");
return FALSE;
}
TSCSIInquiry SCSIInquiry;
SCSIInquiry.OperationCode = SCSI_OP_INQUIRY;
SCSIInquiry.LogicalUnitNumberEVPD = 0;
SCSIInquiry.PageCode = 0;
SCSIInquiry.Reserved = 0;
SCSIInquiry.AllocationLength = sizeof (TSCSIInquiryResponse);
SCSIInquiry.Control = SCSI_CONTROL;
TSCSIInquiryResponse SCSIInquiryResponse;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSIInquiry, sizeof SCSIInquiry,
&SCSIInquiryResponse, sizeof SCSIInquiryResponse,
TRUE) != (int) sizeof SCSIInquiryResponse)
{
LogWrite (FromUmsd, LOG_ERROR, "Device does not respond");
return FALSE;
}
if (SCSIInquiryResponse.PeripheralDeviceType != SCSI_PDT_DIRECT_ACCESS_BLOCK)
{
LogWrite (FromUmsd, LOG_ERROR, "Unsupported device type: 0x%02X", (unsigned) SCSIInquiryResponse.PeripheralDeviceType);
return FALSE;
}
unsigned nTries = 100;
while (--nTries)
{
TSCSITestUnitReady SCSITestUnitReady;
SCSITestUnitReady.OperationCode = SCSI_OP_TEST_UNIT_READY;
SCSITestUnitReady.Reserved = 0;
SCSITestUnitReady.Control = SCSI_CONTROL;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSITestUnitReady,
sizeof SCSITestUnitReady, 0, 0, FALSE) >= 0)
{
break;
}
TSCSIRequestSense SCSIRequestSense;
SCSIRequestSense.OperationCode = SCSI_REQUEST_SENSE;
SCSIRequestSense.DescriptorFormat = 0;
SCSIRequestSense.Reserved1 = 0;
SCSIRequestSense.Reserved2 = 0;
SCSIRequestSense.AllocationLength = sizeof (TSCSIRequestSenseResponse7x);
SCSIRequestSense.Control = SCSI_CONTROL;
TSCSIRequestSenseResponse7x SCSIRequestSenseResponse7x;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSIRequestSense, sizeof SCSIRequestSense,
&SCSIRequestSenseResponse7x, sizeof SCSIRequestSenseResponse7x,
TRUE) < 0)
{
LogWrite (FromUmsd, LOG_ERROR, "Request sense failed");
return FALSE;
}
MsDelay (100);
}
if (nTries == 0)
{
LogWrite (FromUmsd, LOG_ERROR, "Unit is not ready");
return FALSE;
}
TSCSIReadCapacity10 SCSIReadCapacity;
SCSIReadCapacity.OperationCode = SCSI_OP_READ_CAPACITY10;
SCSIReadCapacity.Obsolete = 0;
SCSIReadCapacity.Reserved1 = 0;
SCSIReadCapacity.LogicalBlockAddress = 0;
SCSIReadCapacity.Reserved2 = 0;
SCSIReadCapacity.PartialMediumIndicator = 0;
SCSIReadCapacity.Reserved3 = 0;
SCSIReadCapacity.Control = SCSI_CONTROL;
TSCSIReadCapacityResponse SCSIReadCapacityResponse;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSIReadCapacity, sizeof SCSIReadCapacity,
&SCSIReadCapacityResponse, sizeof SCSIReadCapacityResponse,
TRUE) != (int) sizeof SCSIReadCapacityResponse)
{
LogWrite (FromUmsd, LOG_ERROR, "Read capacity failed");
return FALSE;
}
unsigned nBlockSize = uspi_le2be32 (SCSIReadCapacityResponse.BlockLengthInBytes);
if (nBlockSize != UMSD_BLOCK_SIZE)
{
LogWrite (FromUmsd, LOG_ERROR, "Unsupported block size: %u", nBlockSize);
return FALSE;
}
pThis->m_nBlockCount = uspi_le2be32 (SCSIReadCapacityResponse.ReturnedLogicalBlockAddress);
if (pThis->m_nBlockCount == (u32) -1)
{
LogWrite (FromUmsd, LOG_ERROR, "Unsupported disk size > 2TB");
return FALSE;
}
pThis->m_nBlockCount++;
LogWrite (FromUmsd, LOG_DEBUG, "Capacity is %u MByte", pThis->m_nBlockCount / (0x100000 / UMSD_BLOCK_SIZE));
TString DeviceName;
String (&DeviceName);
StringFormat (&DeviceName, "umsd%u", s_nDeviceNumber++);
DeviceNameServiceAddDevice (DeviceNameServiceGet (), StringGet (&DeviceName), pThis, TRUE);
_String (&DeviceName);
return TRUE;
}
int USBBulkOnlyMassStorageDeviceRead (TUSBBulkOnlyMassStorageDevice *pThis, void *pBuffer, unsigned nCount)
{
assert (pThis != 0);
unsigned nTries = 4;
int nResult;
do
{
nResult = USBBulkOnlyMassStorageDeviceTryRead (pThis, pBuffer, nCount);
if (nResult != (int) nCount)
{
int nStatus = USBBulkOnlyMassStorageDeviceReset (pThis);
if (nStatus != 0)
{
return nStatus;
}
}
}
while ( nResult != (int) nCount
&& --nTries > 0);
return nResult;
}
int USBBulkOnlyMassStorageDeviceWrite (TUSBBulkOnlyMassStorageDevice *pThis, const void *pBuffer, unsigned nCount)
{
assert (pThis != 0);
unsigned nTries = 4;
int nResult;
do
{
nResult = USBBulkOnlyMassStorageDeviceTryWrite (pThis, pBuffer, nCount);
if (nResult != (int) nCount)
{
int nStatus = USBBulkOnlyMassStorageDeviceReset (pThis);
if (nStatus != 0)
{
return nStatus;
}
}
}
while ( nResult != (int) nCount
&& --nTries > 0);
return nResult;
}
unsigned long long USBBulkOnlyMassStorageDeviceSeek (TUSBBulkOnlyMassStorageDevice *pThis, unsigned long long ullOffset)
{
assert (pThis != 0);
pThis->m_ullOffset = ullOffset;
return pThis->m_ullOffset;
}
unsigned USBBulkOnlyMassStorageDeviceGetCapacity (TUSBBulkOnlyMassStorageDevice *pThis)
{
assert (pThis != 0);
return pThis->m_nBlockCount;
}
int USBBulkOnlyMassStorageDeviceTryRead (TUSBBulkOnlyMassStorageDevice *pThis, void *pBuffer, unsigned nCount)
{
assert (pThis != 0);
assert (pBuffer != 0);
if ( (pThis->m_ullOffset & UMSD_BLOCK_MASK) != 0
|| pThis->m_ullOffset > UMSD_MAX_OFFSET)
{
return -1;
}
unsigned nBlockAddress = (unsigned) (pThis->m_ullOffset >> UMSD_BLOCK_SHIFT);
if ((nCount & UMSD_BLOCK_MASK) != 0)
{
return -1;
}
unsigned short usTransferLength = (unsigned short) (nCount >> UMSD_BLOCK_SHIFT);
//LogWrite (FromUmsd, LOG_DEBUG, "TryRead %u/0x%X/%u", nBlockAddress, (unsigned) pBuffer, (unsigned) usTransferLength);
TSCSIRead10 SCSIRead;
SCSIRead.OperationCode = SCSI_OP_READ;
SCSIRead.Reserved1 = 0;
SCSIRead.LogicalBlockAddress = uspi_le2be32 (nBlockAddress);
SCSIRead.Reserved2 = 0;
SCSIRead.TransferLength = uspi_le2be16 (usTransferLength);
SCSIRead.Control = SCSI_CONTROL;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSIRead, sizeof SCSIRead,
pBuffer, nCount,
TRUE) != (int) nCount)
{
LogWrite (FromUmsd, LOG_ERROR, "TryRead failed");
return -1;
}
return nCount;
}
int USBBulkOnlyMassStorageDeviceTryWrite (TUSBBulkOnlyMassStorageDevice *pThis, const void *pBuffer, unsigned nCount)
{
assert (pThis != 0);
assert (pBuffer != 0);
if ( (pThis->m_ullOffset & UMSD_BLOCK_MASK) != 0
|| pThis->m_ullOffset > UMSD_MAX_OFFSET)
{
return -1;
}
unsigned nBlockAddress = (unsigned) (pThis->m_ullOffset >> UMSD_BLOCK_SHIFT);
if ((nCount & UMSD_BLOCK_MASK) != 0)
{
return -1;
}
unsigned short usTransferLength = (unsigned short) (nCount >> UMSD_BLOCK_SHIFT);
//LogWrite (FromUmsd, LOG_DEBUG, "TryWrite %u/0x%X/%u", nBlockAddress, (unsigned) pBuffer, (unsigned) usTransferLength);
TSCSIWrite10 SCSIWrite;
SCSIWrite.OperationCode = SCSI_OP_WRITE;
SCSIWrite.Flags = SCSI_WRITE_FUA;
SCSIWrite.LogicalBlockAddress = uspi_le2be32 (nBlockAddress);
SCSIWrite.Reserved = 0;
SCSIWrite.TransferLength = uspi_le2be16 (usTransferLength);
SCSIWrite.Control = SCSI_CONTROL;
if (USBBulkOnlyMassStorageDeviceCommand (pThis, &SCSIWrite, sizeof SCSIWrite,
(void *) pBuffer, nCount,
FALSE) < 0)
{
LogWrite (FromUmsd, LOG_ERROR, "TryWrite failed");
return -1;
}
return nCount;
}
int USBBulkOnlyMassStorageDeviceCommand (TUSBBulkOnlyMassStorageDevice *pThis,
void *pCmdBlk, unsigned nCmdBlkLen,
void *pBuffer, unsigned nBufLen, boolean bIn)
{
assert (pThis != 0);
assert (pCmdBlk != 0);
assert (6 <= nCmdBlkLen && nCmdBlkLen <= 16);
assert (nBufLen == 0 || pBuffer != 0);
TCBW CBW;
memset (&CBW, 0, sizeof CBW);
CBW.dCWBSignature = CBWSIGNATURE;
CBW.dCWBTag = ++pThis->m_nCWBTag;
CBW.dCBWDataTransferLength = nBufLen;
CBW.bmCBWFlags = bIn ? CBWFLAGS_DATA_IN : 0;
CBW.bCBWLUN = CBWLUN;
CBW.bCBWCBLength = (u8) nCmdBlkLen;
memcpy (CBW.CBWCB, pCmdBlk, nCmdBlkLen);
TUSBHostController *pHost = USBDeviceGetHost (&pThis->m_USBDevice);
assert (pHost != 0);
if (DWHCIDeviceTransfer (pHost, pThis->m_pEndpointOut, &CBW, sizeof CBW) < 0)
{
LogWrite (FromUmsd, LOG_ERROR, "CBW transfer failed");
return -1;
}
int nResult = 0;
if (nBufLen > 0)
{
nResult = DWHCIDeviceTransfer (pHost, bIn ? pThis->m_pEndpointIn : pThis->m_pEndpointOut, pBuffer, nBufLen);
if (nResult < 0)
{
LogWrite (FromUmsd, LOG_ERROR, "Data transfer failed");
return -1;
}
}
TCSW CSW;
if (DWHCIDeviceTransfer (pHost, pThis->m_pEndpointIn, &CSW, sizeof CSW) != (int) sizeof CSW)
{
LogWrite (FromUmsd, LOG_ERROR, "CSW transfer failed");
return -1;
}
if (CSW.dCSWSignature != CSWSIGNATURE)
{
LogWrite (FromUmsd, LOG_ERROR, "CSW signature is wrong");
return -1;
}
if (CSW.dCSWTag != pThis->m_nCWBTag)
{
LogWrite (FromUmsd, LOG_ERROR, "CSW tag is wrong");
return -1;
}
if (CSW.bCSWStatus != CSWSTATUS_PASSED)
{
return -1;
}
if (CSW.dCSWDataResidue != 0)
{
LogWrite (FromUmsd, LOG_ERROR, "Data residue is not 0");
return -1;
}
return nResult;
}
int USBBulkOnlyMassStorageDeviceReset (TUSBBulkOnlyMassStorageDevice *pThis)
{
assert (pThis != 0);
TUSBHostController *pHost = USBDeviceGetHost (&pThis->m_USBDevice);
assert (pHost != 0);
if (DWHCIDeviceControlMessage (pHost, USBDeviceGetEndpoint0 (&pThis->m_USBDevice), 0x21, 0xFF, 0, 0x00, 0, 0) < 0)
{
LogWrite (FromUmsd, LOG_DEBUG, "Cannot reset device");
return -1;
}
if (DWHCIDeviceControlMessage (pHost, USBDeviceGetEndpoint0 (&pThis->m_USBDevice), 0x02, 1, 0, 1, 0, 0) < 0)
{
LogWrite (FromUmsd, LOG_DEBUG, "Cannot clear halt on endpoint 1");
return -1;
}
if (DWHCIDeviceControlMessage (pHost, USBDeviceGetEndpoint0 (&pThis->m_USBDevice), 0x02, 1, 0, 2, 0, 0) < 0)
{
LogWrite (FromUmsd, LOG_DEBUG, "Cannot clear halt on endpoint 2");
return -1;
}
USBEndpointResetPID (pThis->m_pEndpointIn);
USBEndpointResetPID (pThis->m_pEndpointOut);
return 0;
}