828 lines
31 KiB
C
828 lines
31 KiB
C
// usb_storage.c - USB mass-storage BOT/SCSI class driver.
|
|
//
|
|
// The storage driver now binds as a USB function driver to an interface PDO
|
|
// (USB\CLASS_08&SUBCLASS_06&PROT_50). It creates a disk FDO above that
|
|
// interface stack and issues BOT transfers through USB URBs. The xHCI HCD
|
|
// still backs the bulk URBs with the existing per-device rings; later stages
|
|
// will make endpoint rings fully pipe-owned and asynchronous.
|
|
#include "usb.h"
|
|
|
|
DRIVER_OBJECT gUsbStorageDriver;
|
|
|
|
static const IO_DEVICE_ID gUsbStorageIds[] = {
|
|
{ IoBusUsb, { "USB\\CLASS_08&SUBCLASS_06&PROT_50", sizeof("USB\\CLASS_08&SUBCLASS_06&PROT_50") - 1 } },
|
|
};
|
|
|
|
static const IO_DRIVER_ID_TABLE gUsbStorageIdTable = {
|
|
gUsbStorageIds,
|
|
sizeof(gUsbStorageIds) / sizeof(gUsbStorageIds[0]),
|
|
};
|
|
|
|
static USB_STORAGE_EXTENSION* UsbStorageFromDiskDevice(DEVICE_OBJECT* device) {
|
|
if (!device || !device->DeviceExtension)
|
|
return (USB_STORAGE_EXTENSION*)0;
|
|
|
|
USB_STORAGE_DISK_EXTENSION* diskExt = (USB_STORAGE_DISK_EXTENSION*)device->DeviceExtension;
|
|
if (diskExt->Kind != UsbStorageExtensionDisk)
|
|
return (USB_STORAGE_EXTENSION*)0;
|
|
return diskExt->Storage;
|
|
}
|
|
|
|
static USB_STORAGE_EXTENSION* UsbStorageFromInterfaceDevice(DEVICE_OBJECT* device) {
|
|
if (!device || !device->DeviceExtension)
|
|
return (USB_STORAGE_EXTENSION*)0;
|
|
|
|
USB_STORAGE_EXTENSION* ext = (USB_STORAGE_EXTENSION*)device->DeviceExtension;
|
|
if (ext->Kind != UsbStorageExtensionInterface)
|
|
return (USB_STORAGE_EXTENSION*)0;
|
|
return ext;
|
|
}
|
|
|
|
static const USB_ENDPOINT_DESCRIPTOR* UsbStorageFindBulkEndpoint(const USB_PDO_EXTENSION* pdoExt, bool wantIn) {
|
|
if (!pdoExt) return (const USB_ENDPOINT_DESCRIPTOR*)0;
|
|
|
|
for (uint8_t i = 0; i < pdoExt->EndpointCount; i++) {
|
|
const USB_ENDPOINT_DESCRIPTOR* ep = &pdoExt->Endpoints[i];
|
|
uint8_t type = ep->bmAttributes & USB_ENDPOINT_ATTR_TYPE_MASK;
|
|
bool in = (ep->bEndpointAddress & USB_ENDPOINT_DIR_IN) != 0;
|
|
if (type == USB_ENDPOINT_TYPE_BULK && in == wantIn)
|
|
return ep;
|
|
}
|
|
return (const USB_ENDPOINT_DESCRIPTOR*)0;
|
|
}
|
|
|
|
static KSTATUS UsbStorageSubmitBulk(USB_STORAGE_EXTENSION* storage,
|
|
bool dataIn,
|
|
PHYSICAL_ADDRESS phys,
|
|
uint32_t length,
|
|
uint32_t* outActual) {
|
|
if (!storage || !storage->LowerDevice || !storage->Device || !phys || length == 0)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
USB_PIPE* pipe = dataIn ? storage->BulkInPipe : storage->BulkOutPipe;
|
|
if (!pipe || !pipe->Open)
|
|
return STATUS_OBJECT_DEAD;
|
|
|
|
USB_URB urb;
|
|
memset(&urb, 0, sizeof(urb));
|
|
urb.Function = UrbFunctionBulkTransfer;
|
|
urb.Direction = dataIn ? UsbTransferIn : UsbTransferOut;
|
|
urb.Device = storage->Device;
|
|
urb.Pipe = pipe;
|
|
urb.Buffer = (void*)0;
|
|
urb.BufferPhys = phys;
|
|
urb.Length = length;
|
|
|
|
KSTATUS st = UsbSubmitUrbSync(storage->LowerDevice, &urb);
|
|
if (outActual)
|
|
*outActual = KSUCCESS(st) ? urb.ActualLength : 0;
|
|
return st;
|
|
}
|
|
|
|
KSTATUS UsbStorageBulkIn(USB_STORAGE_EXTENSION* storage,
|
|
PHYSICAL_ADDRESS phys,
|
|
uint32_t length,
|
|
uint32_t* outActual) {
|
|
return UsbStorageSubmitBulk(storage, true, phys, length, outActual);
|
|
}
|
|
|
|
KSTATUS UsbStorageBulkOut(USB_STORAGE_EXTENSION* storage,
|
|
PHYSICAL_ADDRESS phys,
|
|
uint32_t length,
|
|
uint32_t* outActual) {
|
|
return UsbStorageSubmitBulk(storage, false, phys, length, outActual);
|
|
}
|
|
|
|
KSTATUS UsbStorageBotCommand(USB_STORAGE_EXTENSION* storage,
|
|
const uint8_t* cdb,
|
|
uint8_t cdbLen,
|
|
void* data,
|
|
PHYSICAL_ADDRESS dataPhys,
|
|
uint32_t dataLength,
|
|
bool dataIn) {
|
|
if (!storage || !storage->XhciDevice || !cdb || cdbLen == 0 || cdbLen > 16)
|
|
return STATUS_INVALID_PARAMETER;
|
|
if (dataLength && (!data || !dataPhys))
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
XHCI_DEVICE* dev = storage->XhciDevice;
|
|
USB_MASS_CBW* cbw = (USB_MASS_CBW*)dev->DescriptorBuffer;
|
|
memset(cbw, 0, sizeof(*cbw));
|
|
cbw->Signature = USB_MASS_BOT_CBW_SIGNATURE;
|
|
cbw->Tag = ++storage->Tag;
|
|
cbw->DataTransferLength = dataLength;
|
|
cbw->Flags = dataIn ? USB_DIR_IN : USB_DIR_OUT;
|
|
cbw->Lun = 0;
|
|
cbw->CommandBlockLength = cdbLen;
|
|
memcpy(cbw->CommandBlock, cdb, cdbLen);
|
|
|
|
uint32_t actual = 0;
|
|
KSTATUS st = UsbStorageBulkOut(storage, dev->DescriptorPhys, sizeof(*cbw), &actual);
|
|
if (!KSUCCESS(st) || actual != sizeof(*cbw))
|
|
return KSUCCESS(st) ? STATUS_DEVICE_ERROR : st;
|
|
|
|
if (dataLength) {
|
|
actual = 0;
|
|
st = dataIn ? UsbStorageBulkIn(storage, dataPhys, dataLength, &actual)
|
|
: UsbStorageBulkOut(storage, dataPhys, dataLength, &actual);
|
|
if (!KSUCCESS(st))
|
|
return st;
|
|
}
|
|
|
|
USB_MASS_CSW* csw = (USB_MASS_CSW*)dev->DescriptorBuffer;
|
|
memset(csw, 0, sizeof(*csw));
|
|
actual = 0;
|
|
st = UsbStorageBulkIn(storage, dev->DescriptorPhys, sizeof(*csw), &actual);
|
|
if (!KSUCCESS(st) || actual != sizeof(*csw))
|
|
return KSUCCESS(st) ? STATUS_DEVICE_ERROR : st;
|
|
|
|
if (csw->Signature != USB_MASS_BOT_CSW_SIGNATURE || csw->Tag != storage->Tag) {
|
|
DbgPrint("usb-storage: invalid CSW slot=%u sig=%08x tag=%08x expected=%08x\n",
|
|
dev->SlotId, csw->Signature, csw->Tag, storage->Tag);
|
|
return STATUS_DEVICE_ERROR;
|
|
}
|
|
if (csw->Status != USB_MASS_BOT_CSW_PASSED) {
|
|
DbgPrint("usb-storage: command %02x failed slot=%u csw_status=%u residue=%u\n",
|
|
cdb[0], dev->SlotId, csw->Status, csw->DataResidue);
|
|
return STATUS_DEVICE_ERROR;
|
|
}
|
|
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
uint32_t UsbBe32(const uint8_t* p) {
|
|
return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) |
|
|
((uint32_t)p[2] << 8) | (uint32_t)p[3];
|
|
}
|
|
|
|
void UsbStoreBe32(uint8_t* p, uint32_t v) {
|
|
p[0] = (uint8_t)(v >> 24);
|
|
p[1] = (uint8_t)(v >> 16);
|
|
p[2] = (uint8_t)(v >> 8);
|
|
p[3] = (uint8_t)v;
|
|
}
|
|
|
|
void UsbStoreBe16(uint8_t* p, uint16_t v) {
|
|
p[0] = (uint8_t)(v >> 8);
|
|
p[1] = (uint8_t)v;
|
|
}
|
|
|
|
KSTATUS UsbStorageInquiry(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage || !storage->XhciDevice)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
XHCI_DEVICE* dev = storage->XhciDevice;
|
|
uint8_t cdb[6] = { SCSI_INQUIRY, 0, 0, 0, 36, 0 };
|
|
memset(dev->MassBuffer, 0, 36);
|
|
KSTATUS st = UsbStorageBotCommand(storage, cdb, sizeof(cdb),
|
|
dev->MassBuffer, dev->MassBufferPhys,
|
|
36, true);
|
|
if (!KSUCCESS(st)) return st;
|
|
|
|
char vendor[9];
|
|
char product[17];
|
|
memcpy(vendor, dev->MassBuffer + 8, 8); vendor[8] = 0;
|
|
memcpy(product, dev->MassBuffer + 16, 16); product[16] = 0;
|
|
DbgPrint("usb-storage: INQUIRY slot=%u vendor='%s' product='%s'\n",
|
|
dev->SlotId, vendor, product);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
KSTATUS UsbStorageRequestSense(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage || !storage->XhciDevice)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
XHCI_DEVICE* dev = storage->XhciDevice;
|
|
uint8_t rs[6] = { SCSI_REQUEST_SENSE, 0, 0, 0, 18, 0 };
|
|
memset(dev->MassBuffer, 0, 18);
|
|
return UsbStorageBotCommand(storage, rs, sizeof(rs),
|
|
dev->MassBuffer, dev->MassBufferPhys, 18, true);
|
|
}
|
|
|
|
KSTATUS UsbStorageTestUnitReady(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage || !storage->XhciDevice)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
uint8_t cdb[6] = { SCSI_TEST_UNIT_READY, 0, 0, 0, 0, 0 };
|
|
KSTATUS last = STATUS_DEVICE_ERROR;
|
|
XHCI_CONTROLLER* xhci = (XHCI_CONTROLLER*)storage->Device->Hcd->ControllerContext;
|
|
|
|
for (uint32_t attempt = 0; attempt < 10; attempt++) {
|
|
KSTATUS st = UsbStorageBotCommand(storage, cdb, sizeof(cdb),
|
|
(void*)0, 0, 0, false);
|
|
if (KSUCCESS(st)) return STATUS_SUCCESS;
|
|
|
|
last = st;
|
|
(void)UsbStorageRequestSense(storage);
|
|
UsbDelayUs(xhci->Hal, 10000u);
|
|
}
|
|
|
|
return last;
|
|
}
|
|
|
|
KSTATUS UsbStorageReadCapacity(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage || !storage->XhciDevice)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
XHCI_DEVICE* dev = storage->XhciDevice;
|
|
uint8_t cdb[10] = { SCSI_READ_CAPACITY_10, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
memset(dev->MassBuffer, 0, 8);
|
|
KSTATUS st = UsbStorageBotCommand(storage, cdb, sizeof(cdb),
|
|
dev->MassBuffer, dev->MassBufferPhys,
|
|
8, true);
|
|
if (!KSUCCESS(st)) return st;
|
|
|
|
uint32_t lastLba = UsbBe32(dev->MassBuffer + 0);
|
|
uint32_t blockSize = UsbBe32(dev->MassBuffer + 4);
|
|
if (blockSize == 0)
|
|
return STATUS_DEVICE_ERROR;
|
|
|
|
storage->BlockSize = blockSize;
|
|
storage->BlockCount = (uint64_t)lastLba + 1ull;
|
|
|
|
dev->MassBlockSize = blockSize;
|
|
dev->MassBlockCount = storage->BlockCount;
|
|
|
|
DbgPrint("usb-storage: capacity slot=%u blocks=%llu block_size=%u (%llu MB)\n",
|
|
dev->SlotId,
|
|
(unsigned long long)storage->BlockCount,
|
|
storage->BlockSize,
|
|
(unsigned long long)((storage->BlockCount * (uint64_t)storage->BlockSize) / (1024ull * 1024ull)));
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
KSTATUS UsbStorageReadWriteSectors(USB_STORAGE_EXTENSION* storage,
|
|
bool write,
|
|
uint64_t lba,
|
|
uint32_t sectors,
|
|
void* buffer) {
|
|
if (!storage || !storage->XhciDevice || !buffer || sectors == 0)
|
|
return STATUS_INVALID_PARAMETER;
|
|
if (storage->BlockSize != USB_STORAGE_SECTOR_SIZE)
|
|
return STATUS_NOT_SUPPORTED;
|
|
if (lba > 0xFFFFFFFFull || sectors > 0xFFFFu)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
XHCI_DEVICE* dev = storage->XhciDevice;
|
|
uint32_t bytes = sectors * USB_STORAGE_SECTOR_SIZE;
|
|
if (bytes > USB_STORAGE_MAX_BYTES)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
uint8_t cdb[10];
|
|
memset(cdb, 0, sizeof(cdb));
|
|
cdb[0] = write ? SCSI_WRITE_10 : SCSI_READ_10;
|
|
UsbStoreBe32(&cdb[2], (uint32_t)lba);
|
|
UsbStoreBe16(&cdb[7], (uint16_t)sectors);
|
|
|
|
if (write)
|
|
memcpy(dev->MassBuffer, buffer, bytes);
|
|
|
|
KSTATUS st = UsbStorageBotCommand(storage, cdb, sizeof(cdb),
|
|
dev->MassBuffer, dev->MassBufferPhys,
|
|
bytes, !write);
|
|
if (!KSUCCESS(st)) return st;
|
|
|
|
if (!write)
|
|
memcpy(buffer, dev->MassBuffer, bytes);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
void UsbStorageAcquireIo(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage) return;
|
|
XHCI_CONTROLLER* xhci = storage->Device ? (XHCI_CONTROLLER*)storage->Device->Hcd->ControllerContext : (XHCI_CONTROLLER*)0;
|
|
while (_InterlockedCompareExchange(&storage->IoBusy, 1, 0) != 0) {
|
|
if (xhci) UsbDelayUs(xhci->Hal, 10);
|
|
}
|
|
}
|
|
|
|
void UsbStorageReleaseIo(USB_STORAGE_EXTENSION* storage) {
|
|
if (storage) _InterlockedExchange(&storage->IoBusy, 0);
|
|
}
|
|
|
|
KSTATUS UsbStorageDispatchReadWrite(DEVICE_OBJECT* device, IRP* irp) {
|
|
if (!device || !irp)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
USB_STORAGE_EXTENSION* storage = UsbStorageFromDiskDevice(device);
|
|
if (!storage || !storage->RemoveLockReady)
|
|
return STATUS_OBJECT_DEAD;
|
|
|
|
KSTATUS st = IoAcquireRemoveLock(&storage->RemoveLock, irp);
|
|
if (!KSUCCESS(st))
|
|
return st;
|
|
|
|
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
|
if (!ios) {
|
|
IoReleaseRemoveLock(&storage->RemoveLock, irp);
|
|
return STATUS_INVALID_PARAMETER;
|
|
}
|
|
if (ios->MajorFunction != IRP_MJ_READ && ios->MajorFunction != IRP_MJ_WRITE) {
|
|
IoReleaseRemoveLock(&storage->RemoveLock, irp);
|
|
return STATUS_INVALID_PARAMETER;
|
|
}
|
|
if ((ios->Parameters.ReadWrite.ByteOffset & 511ull) ||
|
|
(ios->Parameters.ReadWrite.Length & 511u) ||
|
|
ios->Parameters.ReadWrite.Length == 0 ||
|
|
!irp->UserBuffer) {
|
|
IoReleaseRemoveLock(&storage->RemoveLock, irp);
|
|
return STATUS_INVALID_PARAMETER;
|
|
}
|
|
|
|
if (!storage->Started || storage->SurpriseRemoved || storage->Removed ||
|
|
storage->BlockSize != USB_STORAGE_SECTOR_SIZE) {
|
|
IoReleaseRemoveLock(&storage->RemoveLock, irp);
|
|
IoCompleteRequest(irp, STATUS_OBJECT_DEAD, 0);
|
|
return STATUS_PENDING;
|
|
}
|
|
|
|
uint8_t* buffer = (uint8_t*)irp->UserBuffer;
|
|
uint64_t lba = ios->Parameters.ReadWrite.ByteOffset >> 9;
|
|
uint32_t remaining = ios->Parameters.ReadWrite.Length >> 9;
|
|
uint32_t done = 0;
|
|
bool write = (ios->MajorFunction == IRP_MJ_WRITE);
|
|
uint32_t maxSectors = USB_STORAGE_MAX_BYTES / USB_STORAGE_SECTOR_SIZE;
|
|
|
|
st = STATUS_SUCCESS;
|
|
UsbStorageAcquireIo(storage);
|
|
while (remaining) {
|
|
uint32_t chunk = remaining;
|
|
if (chunk > maxSectors) chunk = maxSectors;
|
|
st = UsbStorageReadWriteSectors(storage, write, lba, chunk,
|
|
buffer + (done * USB_STORAGE_SECTOR_SIZE));
|
|
if (!KSUCCESS(st))
|
|
break;
|
|
lba += chunk;
|
|
done += chunk;
|
|
remaining -= chunk;
|
|
}
|
|
UsbStorageReleaseIo(storage);
|
|
|
|
IoReleaseRemoveLock(&storage->RemoveLock, irp);
|
|
IoCompleteRequest(irp, st, KSUCCESS(st) ? (size_t)done * USB_STORAGE_SECTOR_SIZE : 0);
|
|
return STATUS_PENDING;
|
|
}
|
|
|
|
KSTATUS XhciAllocateMassStorageMemory(XHCI_CONTROLLER* xhci, XHCI_DEVICE* dev) {
|
|
if (dev->MassInRing && dev->MassOutRing && dev->MassBuffer)
|
|
return STATUS_SUCCESS;
|
|
|
|
dev->MassInRing = (XHCI_TRB*)UsbDmaAllocPages(xhci, 1, &dev->MassInRingPhys);
|
|
dev->MassOutRing = (XHCI_TRB*)UsbDmaAllocPages(xhci, 1, &dev->MassOutRingPhys);
|
|
dev->MassBuffer = (uint8_t*)UsbDmaAllocPages(xhci, USB_STORAGE_DMA_PAGES, &dev->MassBufferPhys);
|
|
if (!dev->MassInRing || !dev->MassOutRing || !dev->MassBuffer)
|
|
return STATUS_NO_MEMORY;
|
|
|
|
dev->MassInEnqueue = 0;
|
|
dev->MassInCycle = 1;
|
|
dev->MassOutEnqueue = 0;
|
|
dev->MassOutCycle = 1;
|
|
dev->MassTag = 0x1000u + dev->SlotId;
|
|
XhciInitialiseTransferRing(dev->MassInRing, dev->MassInRingPhys, XHCI_BULK_RING_TRBS, dev->MassInCycle);
|
|
XhciInitialiseTransferRing(dev->MassOutRing, dev->MassOutRingPhys, XHCI_BULK_RING_TRBS, dev->MassOutCycle);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
KSTATUS XhciConfigureMassStorageEndpoints(XHCI_CONTROLLER* xhci, XHCI_DEVICE* dev) {
|
|
if (!dev->MassCandidate)
|
|
return STATUS_SUCCESS;
|
|
|
|
KSTATUS st = XhciAllocateMassStorageMemory(xhci, dev);
|
|
if (!KSUCCESS(st)) return st;
|
|
|
|
uint8_t inDci = dev->MassBulkInEndpointId;
|
|
uint8_t outDci = dev->MassBulkOutEndpointId;
|
|
if (inDci < 2 || inDci > 31 || outDci < 2 || outDci > 31)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
memset(dev->InputContext, 0, PAGE_SIZE);
|
|
uint32_t* icc = XhciInputControlContext(xhci, dev);
|
|
uint32_t* slot = XhciInputSlotContext(xhci, dev);
|
|
uint32_t* epIn = XhciInputEndpointContext(xhci, dev, inDci);
|
|
uint32_t* epOut = XhciInputEndpointContext(xhci, dev, outDci);
|
|
|
|
icc[0] = 0;
|
|
icc[1] = (1u << 0) | (1u << inDci) | (1u << outDci);
|
|
|
|
uint8_t maxDci = inDci > outDci ? inDci : outDci;
|
|
slot[0] = XHCI_CONTEXT_ENTRIES(maxDci) | XHCI_SLOT_SPEED(dev->Speed);
|
|
slot[1] = XHCI_SLOT_ROOT_PORT(dev->Port);
|
|
slot[2] = 0;
|
|
slot[3] = 0;
|
|
|
|
uint16_t inMaxPacket = dev->MassBulkInMaxPacket & 0x07FFu;
|
|
uint16_t outMaxPacket = dev->MassBulkOutMaxPacket & 0x07FFu;
|
|
if (inMaxPacket == 0) inMaxPacket = 512;
|
|
if (outMaxPacket == 0) outMaxPacket = 512;
|
|
|
|
epIn[0] = 0;
|
|
epIn[1] = XHCI_EP_CERR_3 | XHCI_EP_TYPE_BULK_IN | ((uint32_t)inMaxPacket << 16);
|
|
epIn[2] = (uint32_t)(dev->MassInRingPhys | XHCI_EP_DCS);
|
|
epIn[3] = (uint32_t)((dev->MassInRingPhys | XHCI_EP_DCS) >> 32);
|
|
epIn[4] = inMaxPacket;
|
|
|
|
epOut[0] = 0;
|
|
epOut[1] = XHCI_EP_CERR_3 | XHCI_EP_TYPE_BULK_OUT | ((uint32_t)outMaxPacket << 16);
|
|
epOut[2] = (uint32_t)(dev->MassOutRingPhys | XHCI_EP_DCS);
|
|
epOut[3] = (uint32_t)((dev->MassOutRingPhys | XHCI_EP_DCS) >> 32);
|
|
epOut[4] = outMaxPacket;
|
|
|
|
XHCI_COMMAND_RESULT result;
|
|
memset(&result, 0, sizeof(result));
|
|
st = XhciSubmitCommand(
|
|
xhci,
|
|
dev->InputContextPhys,
|
|
0,
|
|
(XHCI_TRB_TYPE_CONFIGURE_ENDPOINT << XHCI_TRB_TYPE_SHIFT) |
|
|
((uint32_t)dev->SlotId << 24),
|
|
&result);
|
|
if (!KSUCCESS(st)) {
|
|
DbgPrint("usb-storage: Configure Endpoint failed slot=%u in=%u out=%u st=0x%x cc=%u\n",
|
|
dev->SlotId, inDci, outDci, st, result.CompletionCode);
|
|
return st;
|
|
}
|
|
|
|
UsbTrace("usb-storage: bulk endpoints configured slot=%u if=%u in=%02x/dci=%u out=%02x/dci=%u\n",
|
|
dev->SlotId,
|
|
dev->MassInterface,
|
|
dev->MassBulkInAddress,
|
|
inDci,
|
|
dev->MassBulkOutAddress,
|
|
outDci);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
uint8_t UsbFindFreeDiskIndex(void) {
|
|
for (uint8_t i = 0; i < 8; i++) {
|
|
if (!IoDiskDevicePresent(i))
|
|
return i;
|
|
}
|
|
return 0xFFu;
|
|
}
|
|
|
|
KSTATUS UsbStorageRegisterDisk(USB_STORAGE_EXTENSION* storage) {
|
|
if (!storage || storage->BlockSize != USB_STORAGE_SECTOR_SIZE || storage->BlockCount == 0)
|
|
return STATUS_NOT_SUPPORTED;
|
|
if (storage->DiskDevice)
|
|
return STATUS_ALREADY_EXISTS;
|
|
|
|
DEVICE_OBJECT* disk = (DEVICE_OBJECT*)0;
|
|
KSTATUS st = IoCreateDevice(&gUsbStorageDriver, sizeof(USB_STORAGE_DISK_EXTENSION), &disk);
|
|
if (!KSUCCESS(st)) return st;
|
|
(void)IoSetDeviceName(disk, SYS_SV_LITERAL("usb-storage-disk"));
|
|
|
|
USB_STORAGE_DISK_EXTENSION* diskExt = (USB_STORAGE_DISK_EXTENSION*)disk->DeviceExtension;
|
|
diskExt->Kind = UsbStorageExtensionDisk;
|
|
diskExt->Storage = storage;
|
|
|
|
disk->u.Disk.SectorCount = storage->BlockCount;
|
|
disk->u.Disk.SectorSize = USB_STORAGE_SECTOR_SIZE;
|
|
disk->AlignmentRequirement = USB_STORAGE_SECTOR_SIZE;
|
|
disk->Flags |= DEVICE_OBJECT_REMOVABLE;
|
|
|
|
uint8_t index = UsbFindFreeDiskIndex();
|
|
if (index == 0xFFu) {
|
|
IoDeleteDevice(disk);
|
|
return STATUS_NO_MEMORY;
|
|
}
|
|
|
|
st = IoRegisterDiskDevice(index, disk);
|
|
if (!KSUCCESS(st)) {
|
|
IoDeleteDevice(disk);
|
|
return st;
|
|
}
|
|
|
|
storage->DiskDevice = disk;
|
|
if (storage->XhciDevice)
|
|
storage->XhciDevice->MassDeviceObject = disk;
|
|
|
|
DbgPrint("usb-storage: registered disk %u slot=%u sectors=%llu\n",
|
|
index,
|
|
storage->Device ? storage->Device->SlotId : 0,
|
|
(unsigned long long)storage->BlockCount);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
static KSTATUS UsbStorageStartDevice(DEVICE_OBJECT* device, USB_STORAGE_EXTENSION* storage) {
|
|
if (!device || !storage || !storage->InterfacePdo || !storage->Device)
|
|
return STATUS_INVALID_PARAMETER;
|
|
if (storage->Started)
|
|
return STATUS_SUCCESS;
|
|
if (storage->SurpriseRemoved || storage->Removed)
|
|
return STATUS_OBJECT_DEAD;
|
|
|
|
USB_PDO_EXTENSION* pdoExt = (USB_PDO_EXTENSION*)storage->InterfacePdo->DeviceExtension;
|
|
if (!pdoExt || pdoExt->Kind != UsbPdoInterface)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
const USB_ENDPOINT_DESCRIPTOR* bulkIn = UsbStorageFindBulkEndpoint(pdoExt, true);
|
|
const USB_ENDPOINT_DESCRIPTOR* bulkOut = UsbStorageFindBulkEndpoint(pdoExt, false);
|
|
if (!bulkIn || !bulkOut)
|
|
return STATUS_NOT_FOUND;
|
|
|
|
XHCI_CONTROLLER* xhci = (XHCI_CONTROLLER*)storage->Device->Hcd->ControllerContext;
|
|
XHCI_DEVICE* xdev = (XHCI_DEVICE*)storage->Device->HcdDeviceContext;
|
|
if (!xhci || !xdev)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
storage->XhciDevice = xdev;
|
|
storage->InterfaceNumber = pdoExt->InterfaceDescriptor.bInterfaceNumber;
|
|
storage->BulkInAddress = bulkIn->bEndpointAddress;
|
|
storage->BulkOutAddress = bulkOut->bEndpointAddress;
|
|
storage->BulkInMaxPacket = bulkIn->wMaxPacketSize;
|
|
storage->BulkOutMaxPacket = bulkOut->wMaxPacketSize;
|
|
storage->Tag = 0x1000u + storage->Device->SlotId;
|
|
|
|
xdev->MassCandidate = true;
|
|
xdev->MassInterface = storage->InterfaceNumber;
|
|
xdev->MassBulkInAddress = storage->BulkInAddress;
|
|
xdev->MassBulkOutAddress = storage->BulkOutAddress;
|
|
xdev->MassBulkInEndpointId = XhciEndpointIdFromAddress(storage->BulkInAddress);
|
|
xdev->MassBulkOutEndpointId = XhciEndpointIdFromAddress(storage->BulkOutAddress);
|
|
xdev->MassBulkInMaxPacket = storage->BulkInMaxPacket;
|
|
xdev->MassBulkOutMaxPacket = storage->BulkOutMaxPacket;
|
|
|
|
KSTATUS st = XhciConfigureMassStorageEndpoints(xhci, xdev);
|
|
if (!KSUCCESS(st)) goto fail;
|
|
|
|
st = UsbCreatePipe(storage->Device,
|
|
UsbPipeBulk,
|
|
storage->BulkInAddress,
|
|
storage->BulkInMaxPacket,
|
|
bulkIn->bInterval,
|
|
&storage->BulkInPipe);
|
|
if (!KSUCCESS(st)) goto fail;
|
|
|
|
st = UsbCreatePipe(storage->Device,
|
|
UsbPipeBulk,
|
|
storage->BulkOutAddress,
|
|
storage->BulkOutMaxPacket,
|
|
bulkOut->bInterval,
|
|
&storage->BulkOutPipe);
|
|
if (!KSUCCESS(st)) goto fail;
|
|
|
|
st = UsbStorageInquiry(storage);
|
|
if (!KSUCCESS(st)) goto fail;
|
|
|
|
st = UsbStorageTestUnitReady(storage);
|
|
if (!KSUCCESS(st)) {
|
|
DbgPrint("usb-storage: TEST UNIT READY failed slot=%u st=0x%x\n", storage->Device->SlotId, st);
|
|
goto fail;
|
|
}
|
|
|
|
st = UsbStorageReadCapacity(storage);
|
|
if (!KSUCCESS(st)) goto fail;
|
|
if (storage->BlockSize != USB_STORAGE_SECTOR_SIZE) {
|
|
DbgPrint("usb-storage: unsupported block size slot=%u size=%u\n",
|
|
storage->Device->SlotId, storage->BlockSize);
|
|
st = STATUS_NOT_SUPPORTED;
|
|
goto fail;
|
|
}
|
|
|
|
// The disk namespace publishes an arrival notification synchronously from
|
|
// IoRegisterDiskDevice(). Mark the storage FDO usable before publishing the
|
|
// disk so filesystem hotplug callbacks can read sector 0 immediately.
|
|
xdev->MassActive = true;
|
|
storage->Started = true;
|
|
device->PnpState = DevicePnpStarted;
|
|
if (storage->InterfacePdo)
|
|
storage->InterfacePdo->PnpState = DevicePnpStarted;
|
|
|
|
st = UsbStorageRegisterDisk(storage);
|
|
if (!KSUCCESS(st)) {
|
|
storage->Started = false;
|
|
device->PnpState = DevicePnpNotStarted;
|
|
if (storage->InterfacePdo)
|
|
storage->InterfacePdo->PnpState = DevicePnpNotStarted;
|
|
goto fail;
|
|
}
|
|
|
|
DbgPrint("usb-storage: FDO bound slot=%u if=%u disk-ready\n",
|
|
storage->Device->SlotId, storage->InterfaceNumber);
|
|
return STATUS_SUCCESS;
|
|
|
|
fail:
|
|
xdev->MassCandidate = false;
|
|
xdev->MassActive = false;
|
|
if (storage->BulkInPipe) {
|
|
UsbDereferencePipe(storage->BulkInPipe);
|
|
storage->BulkInPipe = (USB_PIPE*)0;
|
|
}
|
|
if (storage->BulkOutPipe) {
|
|
UsbDereferencePipe(storage->BulkOutPipe);
|
|
storage->BulkOutPipe = (USB_PIPE*)0;
|
|
}
|
|
DbgPrint("usb-storage: start failed slot=%u if=%u st=0x%x\n",
|
|
storage->Device ? storage->Device->SlotId : 0,
|
|
storage->InterfaceNumber,
|
|
st);
|
|
return st;
|
|
}
|
|
|
|
static KSTATUS UsbStorageDiskPnp(DEVICE_OBJECT* device, IRP* irp, USB_STORAGE_EXTENSION* storage, IO_PNP_MINOR_FUNCTION minor) {
|
|
if (!device || !irp || !storage)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
switch (minor) {
|
|
case IRP_MN_START_DEVICE:
|
|
device->PnpState = DevicePnpStarted;
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
|
|
case IRP_MN_QUERY_REMOVE_DEVICE:
|
|
device->PnpState = DevicePnpRemovePending;
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
|
|
case IRP_MN_CANCEL_REMOVE_DEVICE:
|
|
device->PnpState = DevicePnpStarted;
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
|
|
case IRP_MN_SURPRISE_REMOVAL:
|
|
storage->SurpriseRemoved = true;
|
|
if (storage->XhciDevice) {
|
|
storage->XhciDevice->MassSurpriseRemoved = true;
|
|
storage->XhciDevice->MassActive = false;
|
|
}
|
|
device->PnpState = DevicePnpSurpriseRemoved;
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
|
|
case IRP_MN_REMOVE_DEVICE:
|
|
storage->Removed = true;
|
|
if (storage->XhciDevice) {
|
|
storage->XhciDevice->MassRemoved = true;
|
|
storage->XhciDevice->MassActive = false;
|
|
if (storage->XhciDevice->MassDeviceObject == device)
|
|
storage->XhciDevice->MassDeviceObject = (DEVICE_OBJECT*)0;
|
|
}
|
|
device->PnpState = DevicePnpDeleted;
|
|
if (storage->DiskDevice == device)
|
|
storage->DiskDevice = (DEVICE_OBJECT*)0;
|
|
IoDeleteDevice(device);
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
|
|
default:
|
|
return STATUS_NOT_SUPPORTED;
|
|
}
|
|
}
|
|
|
|
static KSTATUS UsbStorageInterfacePnp(DEVICE_OBJECT* device, IRP* irp, USB_STORAGE_EXTENSION* storage, IO_PNP_MINOR_FUNCTION minor) {
|
|
if (!device || !irp || !storage)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
KSTATUS st = STATUS_SUCCESS;
|
|
switch (minor) {
|
|
case IRP_MN_START_DEVICE:
|
|
st = UsbStorageStartDevice(device, storage);
|
|
if (!KSUCCESS(st)) {
|
|
IoCompleteRequest(irp, st, 0);
|
|
return STATUS_PENDING;
|
|
}
|
|
return IoForwardIrpToLower(device, irp);
|
|
|
|
case IRP_MN_QUERY_REMOVE_DEVICE:
|
|
device->PnpState = DevicePnpRemovePending;
|
|
return IoForwardIrpToLower(device, irp);
|
|
|
|
case IRP_MN_CANCEL_REMOVE_DEVICE:
|
|
if (!storage->SurpriseRemoved)
|
|
device->PnpState = DevicePnpStarted;
|
|
return IoForwardIrpToLower(device, irp);
|
|
|
|
case IRP_MN_SURPRISE_REMOVAL:
|
|
storage->SurpriseRemoved = true;
|
|
storage->Started = false;
|
|
if (storage->XhciDevice) {
|
|
storage->XhciDevice->MassSurpriseRemoved = true;
|
|
storage->XhciDevice->MassActive = false;
|
|
}
|
|
if (storage->DiskDevice)
|
|
(void)IoNotifyDiskSurpriseRemoved(storage->DiskDevice->u.Disk.DiskIndex);
|
|
device->PnpState = DevicePnpSurpriseRemoved;
|
|
return IoForwardIrpToLower(device, irp);
|
|
|
|
case IRP_MN_REMOVE_DEVICE: {
|
|
storage->Removed = true;
|
|
storage->Started = false;
|
|
if (storage->DiskDevice)
|
|
(void)IoUnregisterDiskDevice(storage->DiskDevice->u.Disk.DiskIndex);
|
|
if (storage->BulkInPipe) {
|
|
UsbDereferencePipe(storage->BulkInPipe);
|
|
storage->BulkInPipe = (USB_PIPE*)0;
|
|
}
|
|
if (storage->BulkOutPipe) {
|
|
UsbDereferencePipe(storage->BulkOutPipe);
|
|
storage->BulkOutPipe = (USB_PIPE*)0;
|
|
}
|
|
if (storage->RemoveLockReady) {
|
|
IoReleaseRemoveLockAndWait(&storage->RemoveLock, irp);
|
|
IoDestroyRemoveLock(&storage->RemoveLock);
|
|
storage->RemoveLockReady = false;
|
|
}
|
|
device->PnpState = DevicePnpDeleted;
|
|
|
|
KSTATUS lower = storage->LowerDevice ? IoForwardIrpToLower(device, irp) : STATUS_SUCCESS;
|
|
if (storage->LowerDevice) {
|
|
IoDetachDevice(device);
|
|
storage->LowerDevice = (DEVICE_OBJECT*)0;
|
|
}
|
|
IoDeleteDevice(device);
|
|
if (lower == STATUS_NOT_SUPPORTED) {
|
|
IoCompleteRequest(irp, STATUS_SUCCESS, 0);
|
|
return STATUS_PENDING;
|
|
}
|
|
if (lower != STATUS_PENDING && !KSUCCESS(lower)) {
|
|
IoCompleteRequest(irp, lower, 0);
|
|
return STATUS_PENDING;
|
|
}
|
|
return lower;
|
|
}
|
|
|
|
default:
|
|
return IoForwardIrpToLower(device, irp);
|
|
}
|
|
}
|
|
|
|
KSTATUS UsbStorageDispatchPnp(DEVICE_OBJECT* device, IRP* irp) {
|
|
if (!device || !irp)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
|
if (!ios || ios->MajorFunction != IRP_MJ_PNP)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
USB_STORAGE_EXTENSION* iface = UsbStorageFromInterfaceDevice(device);
|
|
if (iface)
|
|
return UsbStorageInterfacePnp(device, irp, iface, (IO_PNP_MINOR_FUNCTION)ios->MinorFunction);
|
|
|
|
USB_STORAGE_EXTENSION* disk = UsbStorageFromDiskDevice(device);
|
|
if (disk)
|
|
return UsbStorageDiskPnp(device, irp, disk, (IO_PNP_MINOR_FUNCTION)ios->MinorFunction);
|
|
|
|
return STATUS_INVALID_PARAMETER;
|
|
}
|
|
|
|
|
|
KSTATUS UsbStorageAddDevice(DRIVER_OBJECT* driver, DEVICE_OBJECT* physicalDeviceObject) {
|
|
if (!driver || !physicalDeviceObject || physicalDeviceObject->Type != DeviceObjectPhysical)
|
|
return STATUS_INVALID_PARAMETER;
|
|
|
|
USB_PDO_EXTENSION* pdoExt = (USB_PDO_EXTENSION*)physicalDeviceObject->DeviceExtension;
|
|
if (!pdoExt || pdoExt->Kind != UsbPdoInterface || !pdoExt->Device)
|
|
return STATUS_INVALID_PARAMETER;
|
|
if (pdoExt->InterfaceDescriptor.bInterfaceClass != USB_CLASS_MASS_STORAGE ||
|
|
pdoExt->InterfaceDescriptor.bInterfaceSubClass != USB_MASS_SUBCLASS_SCSI ||
|
|
pdoExt->InterfaceDescriptor.bInterfaceProtocol != USB_MASS_PROTOCOL_BOT)
|
|
return STATUS_INVALID_PARAMETER;
|
|
if (!UsbStorageFindBulkEndpoint(pdoExt, true) || !UsbStorageFindBulkEndpoint(pdoExt, false))
|
|
return STATUS_NOT_FOUND;
|
|
|
|
DEVICE_OBJECT* fdo = (DEVICE_OBJECT*)0;
|
|
KSTATUS st = IoCreateDevice(driver, sizeof(USB_STORAGE_EXTENSION), &fdo);
|
|
if (!KSUCCESS(st))
|
|
return st;
|
|
(void)IoSetDeviceName(fdo, SYS_SV_LITERAL("usb-storage"));
|
|
|
|
USB_STORAGE_EXTENSION* ext = (USB_STORAGE_EXTENSION*)fdo->DeviceExtension;
|
|
ext->Kind = UsbStorageExtensionInterface;
|
|
ext->Self = fdo;
|
|
ext->InterfacePdo = physicalDeviceObject;
|
|
ext->Device = pdoExt->Device;
|
|
ext->XhciDevice = (XHCI_DEVICE*)pdoExt->Device->HcdDeviceContext;
|
|
ext->InterfaceNumber = pdoExt->InterfaceDescriptor.bInterfaceNumber;
|
|
ext->IoBusy = 0;
|
|
|
|
st = IoInitializeRemoveLock(&ext->RemoveLock);
|
|
if (!KSUCCESS(st)) {
|
|
IoDeleteDevice(fdo);
|
|
return st;
|
|
}
|
|
ext->RemoveLockReady = true;
|
|
|
|
ext->LowerDevice = IoAttachDeviceToDeviceStack(fdo, physicalDeviceObject);
|
|
if (!ext->LowerDevice) {
|
|
IoDestroyRemoveLock(&ext->RemoveLock);
|
|
ext->RemoveLockReady = false;
|
|
IoDeleteDevice(fdo);
|
|
return STATUS_FAILURE;
|
|
}
|
|
|
|
DbgPrint("usb-storage: FDO attached slot=%u if=%u\n",
|
|
ext->Device->SlotId,
|
|
pdoExt->InterfaceDescriptor.bInterfaceNumber);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
KSTATUS UsbInitializeStorageDriver(void) {
|
|
memset(&gUsbStorageDriver, 0, sizeof(gUsbStorageDriver));
|
|
IoInitializeDriverObject(&gUsbStorageDriver, SYS_SV_LITERAL("usb-storage"));
|
|
gUsbStorageDriver.IdTable = &gUsbStorageIdTable;
|
|
gUsbStorageDriver.DriverAddDevice = UsbStorageAddDevice;
|
|
gUsbStorageDriver.MajorFunction[IRP_MJ_READ] = UsbStorageDispatchReadWrite;
|
|
gUsbStorageDriver.MajorFunction[IRP_MJ_WRITE] = UsbStorageDispatchReadWrite;
|
|
gUsbStorageDriver.MajorFunction[IRP_MJ_PNP] = UsbStorageDispatchPnp;
|
|
return IoRegisterDriverObject(&gUsbStorageDriver);
|
|
}
|