[janderwald] 55659: [USBSTOR] - Implement timer routine which checks for hung srbs, not yet activated - Stop timer when the device is stopped - Implement function to check if the attached device is...

Author: janderwald
Date: Fri Feb 17 03:02:14 2012
New Revision: 55659

URL: http://svn.reactos.org/svn/reactos?rev=55659&view=rev
Log:
[USBSTOR]
- Implement timer routine which checks for hung srbs, not yet activated
- Stop timer when the device is stopped
- Implement function to check if the attached device is a floppy device
- Implement function to send read format capacity request
- Implement missing hardware id / device id support for floppy
- Add retry count parameter for debugging purposes
- Tested in VBox + VmWare + RH

Modified:
    trunk/reactos/drivers/usb/usbstor/error.c
    trunk/reactos/drivers/usb/usbstor/fdo.c
    trunk/reactos/drivers/usb/usbstor/misc.c
    trunk/reactos/drivers/usb/usbstor/pdo.c
    trunk/reactos/drivers/usb/usbstor/queue.c
    trunk/reactos/drivers/usb/usbstor/scsi.c
    trunk/reactos/drivers/usb/usbstor/usbstor.c
    trunk/reactos/drivers/usb/usbstor/usbstor.h

Modified: trunk/reactos/drivers/usb/usbstor/error.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/error.…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/error.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/error.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -112,10 +112,8 @@
 {
     NTSTATUS Status = STATUS_SUCCESS;
     PIO_STACK_LOCATION Stack;
-    //USBD_PIPE_HANDLE PipeHandle;
     PSCSI_REQUEST_BLOCK Request;
     PCDB pCDB;
-
 
     //
     // first perform a mass storage reset step 1 in 5.3.4 USB Mass Storage Bulk Only
Specification
@@ -172,10 +170,20 @@
            
USBSTOR_QueueTerminateRequest(Context->PDODeviceExtension->LowerDeviceObject,
Context->Irp);
             IoCompleteRequest(Context->Irp, IO_NO_INCREMENT);
 
-             //
+            //
             // Start the next request
             //
            
USBSTOR_QueueNextRequest(Context->PDODeviceExtension->LowerDeviceObject);
+
+            //
+            // srb handling finished
+            //
+            Context->FDODeviceExtension->SrbErrorHandlingActive = FALSE;
+
+            //
+            // clear timer srb
+            //
+            Context->FDODeviceExtension->LastTimerActiveSrb = NULL;
         }
     }
     else
@@ -187,13 +195,42 @@
             //
             ASSERT(Context->Event);
             KeSetEvent(Context->Event, 0, FALSE);
+
+            //
+            // srb handling finished
+            //
+            Context->FDODeviceExtension->SrbErrorHandlingActive = FALSE;
+
+            //
+            // clear timer srb
+            //
+            Context->FDODeviceExtension->LastTimerActiveSrb = NULL;
         }
     }
 
     if (NT_SUCCESS(Status))
     {
-        DPRINT1("Retrying\n");
-        USBSTOR_HandleExecuteSCSI(*Context->PDODeviceExtension->PDODeviceObject,
Context->Irp);
+        DPRINT1("Retrying Count %x\n", Context->RetryCount);
+
+        //
+        // re-schedule request
+        //
+        USBSTOR_HandleExecuteSCSI(Context->PDODeviceExtension->Self,
Context->Irp, Context->RetryCount + 1);
+
+        //
+        // srb error handling finished
+        //
+        Context->FDODeviceExtension->SrbErrorHandlingActive = FALSE;
+
+        //
+        // srb error handling finished
+        //
+        Context->FDODeviceExtension->TimerWorkQueueEnabled = TRUE;
+
+        //
+        // clear timer srb
+        //
+        Context->FDODeviceExtension->LastTimerActiveSrb = NULL;
     }
 
     //
@@ -213,8 +250,6 @@
     PVOID Context)
 {
     NTSTATUS Status;
-    PIO_STACK_LOCATION IoStack;
-
     PERRORHANDLER_WORKITEM_DATA WorkItemData = (PERRORHANDLER_WORKITEM_DATA)Context;
 
     //
@@ -224,42 +259,9 @@
     DPRINT1("USBSTOR_ResetPipeWithHandle Status %x\n", Status);
 
     //
-    // get next stack location
-    //
-
-    IoStack = IoGetNextIrpStackLocation(WorkItemData->Irp);
-
-    //
-    // now initialize the urb for sending the csw
-    //
-    UsbBuildInterruptOrBulkTransferRequest(&WorkItemData->Context->Urb,
-                                           sizeof(struct
_URB_BULK_OR_INTERRUPT_TRANSFER),
-                                          
WorkItemData->Context->FDODeviceExtension->InterfaceInformation->Pipes[WorkItemData->Context->FDODeviceExtension->BulkInPipeIndex].PipeHandle,
-                                           WorkItemData->Context->csw,
-                                           NULL,
-                                           512, //FIXME
-                                           USBD_TRANSFER_DIRECTION_IN |
USBD_SHORT_TRANSFER_OK,
-                                           NULL);
-
-    //
-    // initialize stack location
-    //
-    IoStack->MajorFunction = IRP_MJ_INTERNAL_DEVICE_CONTROL;
-    IoStack->Parameters.DeviceIoControl.IoControlCode =
IOCTL_INTERNAL_USB_SUBMIT_URB;
-    IoStack->Parameters.Others.Argument1 =
(PVOID)&WorkItemData->Context->Urb;
-    IoStack->Parameters.DeviceIoControl.InputBufferLength =
WorkItemData->Context->Urb.UrbHeader.Length;
-    WorkItemData->Irp->IoStatus.Status = STATUS_SUCCESS;
-
-
-    //
-    // setup completion routine
-    //
-    IoSetCompletionRoutine(WorkItemData->Irp, USBSTOR_CSWCompletionRoutine, Context,
TRUE, TRUE, TRUE);
-
-    //
-    // call driver
-    //
-    IoCallDriver(WorkItemData->Context->FDODeviceExtension->LowerDeviceObject,
WorkItemData->Irp);
+    // now resend the csw as the stall got cleared
+    //
+    USBSTOR_SendCSW(WorkItemData->Context, WorkItemData->Irp);
 }
 
 VOID
@@ -268,8 +270,14 @@
     PVOID Context)
 {
     NTSTATUS Status;
+    PFDO_DEVICE_EXTENSION FDODeviceExtension;
     PERRORHANDLER_WORKITEM_DATA WorkItemData = (PERRORHANDLER_WORKITEM_DATA)Context;
 
+    //
+    // get fdo
+    //
+    FDODeviceExtension = WorkItemData->Context->FDODeviceExtension;
+
     if (WorkItemData->Context->ErrorIndex == 2)
     {
         //
@@ -290,3 +298,139 @@
     //
     ExFreePool(WorkItemData);
 }
+
+VOID
+NTAPI
+USBSTOR_TimerWorkerRoutine(
+    IN PVOID Context)
+{
+    PFDO_DEVICE_EXTENSION FDODeviceExtension;
+    NTSTATUS Status;
+    PERRORHANDLER_WORKITEM_DATA WorkItemData = (PERRORHANDLER_WORKITEM_DATA)Context;
+
+    //
+    // get device extension
+    //
+    FDODeviceExtension =
(PFDO_DEVICE_EXTENSION)WorkItemData->DeviceObject->DeviceExtension;
+    ASSERT(FDODeviceExtension->Common.IsFDO);
+
+    //
+    // first perform a mass storage reset step 1 in 5.3.4 USB Mass Storage Bulk Only
Specification
+    //
+    Status = USBSTOR_ResetDevice(FDODeviceExtension->LowerDeviceObject,
FDODeviceExtension);
+    if (NT_SUCCESS(Status))
+    {
+        //
+        // step 2 reset bulk in pipe section 5.3.4
+        //
+        Status = USBSTOR_ResetPipeWithHandle(FDODeviceExtension->LowerDeviceObject,
FDODeviceExtension->InterfaceInformation->Pipes[FDODeviceExtension->BulkInPipeIndex].PipeHandle);
+        if (NT_SUCCESS(Status))
+        {
+            //
+            // finally reset bulk out pipe
+            //
+            Status =
USBSTOR_ResetPipeWithHandle(FDODeviceExtension->LowerDeviceObject,
FDODeviceExtension->InterfaceInformation->Pipes[FDODeviceExtension->BulkOutPipeIndex].PipeHandle);
+        }
+    }
+    DPRINT1("Status %x\n", Status);
+
+    //
+    // clear timer srb
+    //
+    FDODeviceExtension->LastTimerActiveSrb = NULL;
+
+    //
+    // re-schedule request
+    //
+    //USBSTOR_HandleExecuteSCSI(WorkItemData->Context->PDODeviceExtension->Self,
WorkItemData->Context->Irp, Context->RetryCount + 1);
+
+
+
+    //
+    // do not retry for the same packet again
+    //
+    FDODeviceExtension->TimerWorkQueueEnabled = FALSE;
+
+    //
+    // Free Work Item Data
+    //
+    ExFreePool(WorkItemData);
+}
+
+
+VOID
+NTAPI
+USBSTOR_TimerRoutine(
+    PDEVICE_OBJECT DeviceObject,
+     PVOID Context)
+{
+    PFDO_DEVICE_EXTENSION FDODeviceExtension;
+    BOOLEAN ResetDevice = FALSE;
+    PERRORHANDLER_WORKITEM_DATA WorkItemData;
+
+    //
+    // get device extension
+    //
+    FDODeviceExtension = (PFDO_DEVICE_EXTENSION)Context;
+    DPRINT1("[USBSTOR] TimerRoutine entered\n");
+    DPRINT1("[USBSTOR] ActiveSrb %p ResetInProgress %x LastTimerActiveSrb
%p\n", FDODeviceExtension->ActiveSrb, FDODeviceExtension->ResetInProgress,
FDODeviceExtension->LastTimerActiveSrb);
+
+    //
+    // acquire spinlock
+    //
+    KeAcquireSpinLockAtDpcLevel(&FDODeviceExtension->IrpListLock);
+
+    //
+    // is there an active srb and no global reset is in progress
+    //
+    if (FDODeviceExtension->ActiveSrb &&
FDODeviceExtension->ResetInProgress == FALSE &&
FDODeviceExtension->TimerWorkQueueEnabled)
+    {
+        if (FDODeviceExtension->LastTimerActiveSrb != NULL &&
FDODeviceExtension->LastTimerActiveSrb == FDODeviceExtension->ActiveSrb)
+        {
+            //
+            // check if empty
+            //
+            DPRINT1("[USBSTOR] ActiveSrb %p hang detected\n",
FDODeviceExtension->ActiveSrb);
+            ResetDevice = TRUE;
+        }
+        else
+        {
+            //
+            // update pointer
+            //
+            FDODeviceExtension->LastTimerActiveSrb =
FDODeviceExtension->ActiveSrb;
+        }
+    }
+    else
+    {
+        //
+        // reset srb
+        //
+        FDODeviceExtension->LastTimerActiveSrb = NULL;
+    }
+
+    //
+    // release lock
+    //
+    KeReleaseSpinLockFromDpcLevel(&FDODeviceExtension->IrpListLock);
+
+
+    if (ResetDevice && FDODeviceExtension->TimerWorkQueueEnabled &&
FDODeviceExtension->SrbErrorHandlingActive == FALSE)
+    {
+        WorkItemData = (PERRORHANDLER_WORKITEM_DATA)ExAllocatePool(NonPagedPool,
sizeof(ERRORHANDLER_WORKITEM_DATA));
+        if (WorkItemData)
+        {
+           //
+           // Initialize and queue the work item to handle the error
+           //
+           ExInitializeWorkItem(&WorkItemData->WorkQueueItem,
+                                 USBSTOR_TimerWorkerRoutine,
+                                 WorkItemData);
+
+           WorkItemData->DeviceObject =
FDODeviceExtension->FunctionalDeviceObject;
+
+           DPRINT1("[USBSTOR] Queing Timer WorkItem\n");
+           ExQueueWorkItem(&WorkItemData->WorkQueueItem, DelayedWorkQueue);
+        }
+     }
+}

Modified: trunk/reactos/drivers/usb/usbstor/fdo.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/fdo.c?…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/fdo.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/fdo.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -300,6 +300,13 @@
     }
 #endif
 
+
+    //
+    // start the timer
+    //
+    //IoStartTimer(DeviceObject);
+
+
     //
     // fdo is now initialized
     //
@@ -341,8 +348,14 @@
        case IRP_MN_STOP_DEVICE:
        {
            DPRINT1("USBSTOR_FdoHandlePnp: IRP_MN_STOP_DEVICE
unimplemented\n");
-           Status = STATUS_NOT_SUPPORTED;
-           break;
+           IoStopTimer(DeviceObject);
+           Irp->IoStatus.Status = STATUS_SUCCESS;
+
+            //
+            // forward irp to next device object
+            //
+            IoSkipCurrentIrpStackLocation(Irp);
+            return IoCallDriver(DeviceExtension->LowerDeviceObject, Irp);
        }
        case IRP_MN_REMOVE_DEVICE:
        {

Modified: trunk/reactos/drivers/usb/usbstor/misc.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/misc.c…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/misc.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/misc.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -418,3 +418,100 @@
     return Status;
 
 }
+
+BOOLEAN
+USBSTOR_IsFloppy(
+    IN PUCHAR Buffer,
+    IN ULONG BufferLength,
+    OUT PUCHAR MediumTypeCode)
+{
+    PUFI_CAPACITY_FORMAT_HEADER FormatHeader;
+    PUFI_CAPACITY_DESCRIPTOR Descriptor;
+    ULONG Length, Index, BlockCount, BlockLength;
+
+    //
+    // get format header
+    //
+    FormatHeader = (PUFI_CAPACITY_FORMAT_HEADER)Buffer;
+
+    //
+    // sanity checks
+    //
+    ASSERT(FormatHeader->Reserved1 == 0x00);
+    ASSERT(FormatHeader->Reserved2 == 0x00);
+    ASSERT(FormatHeader->Reserved3 == 0x00);
+
+    //
+    // is there capacity data
+    //
+    if (!FormatHeader->CapacityLength)
+    {
+        //
+        // no data provided
+        //
+        DPRINT1("[USBSTOR] No capacity length\n");
+        return FALSE;
+    }
+
+    //
+    // the format header are always 8 bytes in length
+    //
+    ASSERT((FormatHeader->CapacityLength & 0x7) == 0);
+    DPRINT1("CapacityLength %x\n", FormatHeader->CapacityLength);
+
+    //
+    // grab length and locate first descriptor
+    //
+    Length = FormatHeader->CapacityLength;
+    Descriptor = (PUFI_CAPACITY_DESCRIPTOR)(FormatHeader + 1);
+    for(Index = 0; Index < Length / sizeof(UFI_CAPACITY_DESCRIPTOR); Index++)
+    {
+        //
+        // blocks are little endian format
+        //
+        BlockCount = NTOHL(Descriptor->BlockCount);
+
+        //
+        // get block length
+        //
+        BlockLength = NTOHL((Descriptor->BlockLengthByte0 << 24 |
Descriptor->BlockLengthByte1 << 16 | Descriptor->BlockLengthByte2 <<
8));
+
+        DPRINT1("BlockCount %x BlockLength %x Code %x\n", BlockCount,
BlockLength, Descriptor->Code);
+
+        if (BlockLength == 512 && BlockCount == 1440)
+        {
+            //
+            // 720 KB DD
+            //
+            *MediumTypeCode = 0x1E;
+            return TRUE;
+        }
+        else if (BlockLength == 1024 && BlockCount == 1232)
+        {
+            //
+            // 1,25 MB
+            //
+            *MediumTypeCode = 0x93;
+            return TRUE;
+        }
+        else if (BlockLength == 512 && BlockCount == 2880)
+        {
+            //
+            // 1,44MB KB DD
+            //
+            *MediumTypeCode = 0x94;
+            return TRUE;
+        }
+
+        //
+        // move to next descriptor
+        //
+        Descriptor = (Descriptor + 1);
+    }
+
+    //
+    // no floppy detected
+    //
+    return FALSE;
+}
+

Modified: trunk/reactos/drivers/usb/usbstor/pdo.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/pdo.c?…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/pdo.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/pdo.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -13,19 +13,24 @@
 
 LPCSTR
 USBSTOR_GetDeviceType(
-    IN PUFI_INQUIRY_RESPONSE InquiryData)
+    IN PUFI_INQUIRY_RESPONSE InquiryData,
+    IN UCHAR IsFloppy)
 {
     //
     // check if device type is zero
     //
     if (InquiryData->DeviceType == 0)
     {
+        if (IsFloppy)
+        {
+            //
+            // floppy device
+            //
+            return "SFloppy";
+        }
+
         //
         // direct access device
-        //
-
-        //
-        // FIXME: check if floppy
         //
         return "Disk";
     }
@@ -82,19 +87,24 @@
 
 LPCSTR
 USBSTOR_GetGenericType(
-    IN PUFI_INQUIRY_RESPONSE InquiryData)
+    IN PUFI_INQUIRY_RESPONSE InquiryData,
+    IN UCHAR IsFloppy)
 {
     //
     // check if device type is zero
     //
     if (InquiryData->DeviceType == 0)
     {
+        if (IsFloppy)
+        {
+            //
+            // floppy device
+            //
+            return "GenSFloppy";
+        }
+
         //
         // direct access device
-        //
-
-        //
-        // FIXME: check if floppy
         //
         return "GenDisk";
     }
@@ -286,7 +296,7 @@
     //
     // get device type
     //
-    DeviceType = USBSTOR_GetDeviceType(InquiryData);
+    DeviceType = USBSTOR_GetDeviceType(InquiryData, DeviceExtension->IsFloppy);
 
     //
     // zero buffer
@@ -434,8 +444,8 @@
     //
     // get device type and generic type
     //
-    DeviceType = USBSTOR_GetDeviceType(InquiryData);
-    GenericType = USBSTOR_GetGenericType(InquiryData);
+    DeviceType = USBSTOR_GetDeviceType(InquiryData, PDODeviceExtension->IsFloppy);
+    GenericType = USBSTOR_GetGenericType(InquiryData, PDODeviceExtension->IsFloppy);
 
     ASSERT(GenericType);
 
@@ -590,7 +600,7 @@
     //
     // get target device type
     //
-    DeviceType =
USBSTOR_GetDeviceType((PUFI_INQUIRY_RESPONSE)PDODeviceExtension->InquiryData);
+    DeviceType =
USBSTOR_GetDeviceType((PUFI_INQUIRY_RESPONSE)PDODeviceExtension->InquiryData,
PDODeviceExtension->IsFloppy);
 
     //
     // zero memory
@@ -984,6 +994,7 @@
     PDODeviceExtension->Common.IsFDO = FALSE;
     PDODeviceExtension->LowerDeviceObject = DeviceObject;
     PDODeviceExtension->PDODeviceObject = ChildDeviceObject;
+    PDODeviceExtension->Self = PDO;
 
     //
     // set device flags
@@ -1000,7 +1011,24 @@
     //
     *ChildDeviceObject = PDO;
 
-    USBSTOR_SendInquiryCmd(PDO);
+    //
+    // send inquiry command
+    //
+    USBSTOR_SendInquiryCmd(PDO, 0);
+
+    //
+    // retrieve format capacity
+    //
+    if (NT_SUCCESS(USBSTOR_SendFormatCapacity(PDO, 0)))
+    {
+        //
+        // check if its a floppy
+        //
+        PDODeviceExtension->IsFloppy =
USBSTOR_IsFloppy(PDODeviceExtension->FormatData, PAGE_SIZE /*FIXME*/,
&PDODeviceExtension->MediumTypeCode);
+        DPRINT1("[USBSTOR] IsFloppy %x MediumTypeCode %x\n",
PDODeviceExtension->IsFloppy, PDODeviceExtension->MediumTypeCode);
+    }
+
+
 
     //
     // done

Modified: trunk/reactos/drivers/usb/usbstor/queue.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/queue.…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/queue.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/queue.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -660,7 +660,7 @@
     //
     // execute scsi
     //
-    Status = USBSTOR_HandleExecuteSCSI(IoStack->DeviceObject, Irp);
+    Status = USBSTOR_HandleExecuteSCSI(IoStack->DeviceObject, Irp, 0);
 
     //
     // FIXME: handle error

Modified: trunk/reactos/drivers/usb/usbstor/scsi.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/scsi.c…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/scsi.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/scsi.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -135,6 +135,16 @@
     }
 
     //
+    // error handling started
+    //
+    Context->FDODeviceExtension->SrbErrorHandlingActive = TRUE;
+
+    //
+    // srb error handling finished
+    //
+    Context->FDODeviceExtension->TimerWorkQueueEnabled = FALSE;
+
+    //
     // Initialize and queue the work item to handle the error
     //
     ExInitializeWorkItem(&ErrorHandlerWorkItemData->WorkQueueItem,
@@ -175,8 +185,6 @@
     NTSTATUS Status;
     PURB Urb;
 
-    DPRINT("USBSTOR_CSWCompletionRoutine Irp %p Ctx %p\n", Irp, Ctx);
-
     //
     // access context
     //
@@ -212,7 +220,7 @@
         }
     }
 
-    DPRINT1("USBSTOR_CSWCompletionRoutine Status %x\n",
Irp->IoStatus.Status);
+    DPRINT("USBSTOR_CSWCompletionRoutine Status %x\n",
Irp->IoStatus.Status);
 
     if (!NT_SUCCESS(Irp->IoStatus.Information))
     {
@@ -382,32 +390,16 @@
     return STATUS_MORE_PROCESSING_REQUIRED;
 }
 
-//
-// driver verifier
-//
-IO_COMPLETION_ROUTINE USBSTOR_DataCompletionRoutine;
-
-NTSTATUS
-NTAPI
-USBSTOR_DataCompletionRoutine(
-    PDEVICE_OBJECT DeviceObject,
-    PIRP Irp, 
-    PVOID Ctx)
-{
-    PIRP_CONTEXT Context;
+VOID
+USBSTOR_SendCSW(
+    PIRP_CONTEXT Context,
+    PIRP Irp)
+{
     PIO_STACK_LOCATION IoStack;
 
-    DPRINT("USBSTOR_DataCompletionRoutine Irp %p Ctx %p\n", Irp, Ctx);
-
-    //
-    // access context
-    //
-    Context = (PIRP_CONTEXT)Ctx;
-
-    //
-    // get next stack location
-    //
-
+    //
+    // get next irp stack location
+    //
     IoStack = IoGetNextIrpStackLocation(Irp);
 
     //
@@ -441,7 +433,52 @@
     // call driver
     //
     IoCallDriver(Context->FDODeviceExtension->LowerDeviceObject, Irp);
-
+}
+
+
+//
+// driver verifier
+//
+IO_COMPLETION_ROUTINE USBSTOR_DataCompletionRoutine;
+
+NTSTATUS
+NTAPI
+USBSTOR_DataCompletionRoutine(
+    PDEVICE_OBJECT DeviceObject,
+    PIRP Irp, 
+    PVOID Ctx)
+{
+    PIRP_CONTEXT Context;
+    NTSTATUS Status;
+
+    DPRINT("USBSTOR_DataCompletionRoutine Irp %p Ctx %p Status %x\n", Irp, Ctx,
Irp->IoStatus.Status);
+
+    //
+    // access context
+    //
+    Context = (PIRP_CONTEXT)Ctx;
+
+#if 0
+    if (!NT_SUCCESS(Irp->IoStatus.Status))
+    {
+        //
+        // clear stall and resend cbw
+        //
+        Context->ErrorIndex = 1;
+        Status = USBSTOR_QueueWorkItem(Context, Irp);
+        ASSERT(Status == STATUS_MORE_PROCESSING_REQUIRED);
+        return STATUS_MORE_PROCESSING_REQUIRED;
+    }
+#endif
+
+    //
+    // send csw
+    //
+    USBSTOR_SendCSW(Context, Irp);
+
+    //
+    // cancel completion
+    //
     return STATUS_MORE_PROCESSING_REQUIRED;
 }
 
@@ -462,7 +499,7 @@
     UCHAR Code;
     USBD_PIPE_HANDLE PipeHandle;
 
-    DPRINT("USBSTOR_CBWCompletionRoutine Irp %p Ctx %p\n", Irp, Ctx);
+    DPRINT("USBSTOR_CBWCompletionRoutine Irp %p Ctx %p Status %x\n", Irp, Ctx,
Irp->IoStatus.Status);
 
     //
     // access context
@@ -566,6 +603,37 @@
 
 }
 
+NTSTATUS
+USBSTOR_SendCBW(
+    PIRP_CONTEXT Context,
+    PIRP Irp)
+{
+    PIO_STACK_LOCATION IoStack;
+
+    //
+    // get next stack location
+    //
+    IoStack = IoGetNextIrpStackLocation(Irp);
+
+    //
+    // initialize stack location
+    //
+    IoStack->MajorFunction = IRP_MJ_INTERNAL_DEVICE_CONTROL;
+    IoStack->Parameters.DeviceIoControl.IoControlCode =
IOCTL_INTERNAL_USB_SUBMIT_URB;
+    IoStack->Parameters.Others.Argument1 = (PVOID)&Context->Urb;
+    IoStack->Parameters.DeviceIoControl.InputBufferLength =
Context->Urb.UrbHeader.Length;
+    Irp->IoStatus.Status = STATUS_SUCCESS;
+
+    //
+    // setup completion routine
+    //
+    IoSetCompletionRoutine(Irp, USBSTOR_CBWCompletionRoutine, Context, TRUE, TRUE,
TRUE);
+
+    //
+    // call driver
+    //
+    return IoCallDriver(Context->FDODeviceExtension->LowerDeviceObject, Irp);
+}
 
 NTSTATUS
 USBSTOR_SendRequest(
@@ -575,13 +643,13 @@
     IN UCHAR CommandLength,
     IN PUCHAR Command,
     IN ULONG TransferDataLength,
-    IN PUCHAR TransferData)
+    IN PUCHAR TransferData,
+    IN ULONG RetryCount)
 {
     PIRP_CONTEXT Context;
     PPDO_DEVICE_EXTENSION PDODeviceExtension;
     PFDO_DEVICE_EXTENSION FDODeviceExtension;
     PIRP Irp;
-    PIO_STACK_LOCATION IoStack;
     PUCHAR MdlVirtualAddress;
 
     //
@@ -640,6 +708,7 @@
     Context->FDODeviceExtension = FDODeviceExtension;
     Context->PDODeviceExtension = PDODeviceExtension;
     Context->Event = Event;
+    Context->RetryCount = RetryCount;
 
     //
     // is there transfer data
@@ -745,25 +814,6 @@
         return STATUS_INSUFFICIENT_RESOURCES;
     }
 
-    //
-    // get next stack location
-    //
-    IoStack = IoGetNextIrpStackLocation(Irp);
-
-    //
-    // initialize stack location
-    //
-    IoStack->MajorFunction = IRP_MJ_INTERNAL_DEVICE_CONTROL;
-    IoStack->Parameters.DeviceIoControl.IoControlCode =
IOCTL_INTERNAL_USB_SUBMIT_URB;
-    IoStack->Parameters.Others.Argument1 = (PVOID)&Context->Urb;
-    IoStack->Parameters.DeviceIoControl.InputBufferLength =
Context->Urb.UrbHeader.Length;
-    Irp->IoStatus.Status = STATUS_SUCCESS;
-
-    //
-    // setup completion routine
-    //
-    IoSetCompletionRoutine(Irp, USBSTOR_CBWCompletionRoutine, Context, TRUE, TRUE,
TRUE);
-
     if (OriginalRequest)
     {
         //
@@ -773,19 +823,84 @@
     }
 
     //
-    // call driver
-    //
-    IoCallDriver(FDODeviceExtension->LowerDeviceObject, Irp);
+    // send request
+    //
+    USBSTOR_SendCBW(Context, Irp);
 
     //
     // done
     //
     return STATUS_PENDING;
+}
+
+NTSTATUS
+USBSTOR_SendFormatCapacity(
+    IN PDEVICE_OBJECT DeviceObject,
+    IN ULONG RetryCount)
+{
+    UFI_READ_FORMAT_CAPACITY Cmd;
+    NTSTATUS Status;
+    KEVENT Event;
+    PPDO_DEVICE_EXTENSION PDODeviceExtension;
+    PUCHAR Response;
+
+
+    //
+    // allocate inquiry response
+    //
+    Response = AllocateItem(NonPagedPool, PAGE_SIZE);
+    if (!Response)
+    {
+        //
+        // no memory
+        //
+        return STATUS_INSUFFICIENT_RESOURCES;
+    }
+
+    //
+    // get PDO device extension
+    //
+    PDODeviceExtension = (PPDO_DEVICE_EXTENSION)DeviceObject->DeviceExtension;
+
+    //
+    // initialize inquiry cmd
+    //
+    RtlZeroMemory(&Cmd, sizeof(UFI_READ_FORMAT_CAPACITY));
+    Cmd.Code = SCSIOP_READ_FORMATTED_CAPACITY;
+    Cmd.LUN = (PDODeviceExtension->LUN & MAX_LUN) << 5;
+    Cmd.AllocationLengthLsb = 0xFC; // FIXME: compute some same value
+
+    //
+    // initialize event
+    //
+    KeInitializeEvent(&Event, NotificationEvent, FALSE);
+
+    //
+    // now send the request
+    //
+    Status = USBSTOR_SendRequest(DeviceObject, NULL, &Event,
UFI_READ_FORMAT_CAPACITY_CMD_LEN, (PUCHAR)&Cmd, sizeof(UFI_INQUIRY_RESPONSE),
(PUCHAR)Response, RetryCount);
+
+    //
+    // wait for the action to complete
+    //
+    KeWaitForSingleObject(&Event, Executive, KernelMode, FALSE, NULL);
+
+    //
+    // store inquiry data
+    //
+    PDODeviceExtension->FormatData = Response;
+
+    //
+    // done
+    //
+    return Status;
+
 }
 
 NTSTATUS
 USBSTOR_SendInquiryCmd(
-    IN PDEVICE_OBJECT DeviceObject)
+    IN PDEVICE_OBJECT DeviceObject,
+    IN ULONG RetryCount)
 {
     UFI_INQUIRY_CMD Cmd;
     NTSTATUS Status;
@@ -827,7 +942,7 @@
     //
     // now send the request
     //
-    Status = USBSTOR_SendRequest(DeviceObject, NULL, &Event, UFI_INQUIRY_CMD_LEN,
(PUCHAR)&Cmd, sizeof(UFI_INQUIRY_RESPONSE), (PUCHAR)Response);
+    Status = USBSTOR_SendRequest(DeviceObject, NULL, &Event, UFI_INQUIRY_CMD_LEN,
(PUCHAR)&Cmd, sizeof(UFI_INQUIRY_RESPONSE), (PUCHAR)Response, RetryCount);
 
     //
     // wait for the action to complete
@@ -863,7 +978,8 @@
 NTSTATUS
 USBSTOR_SendCapacityCmd(
     IN PDEVICE_OBJECT DeviceObject,
-    IN PIRP Irp)
+    IN PIRP Irp,
+    IN ULONG RetryCount)
 {
     UFI_CAPACITY_CMD Cmd;
     PUFI_CAPACITY_RESPONSE Response;
@@ -904,13 +1020,14 @@
     //
     // send request, response will be freed in completion routine
     //
-    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_READ_CAPACITY_CMD_LEN,
(PUCHAR)&Cmd, sizeof(UFI_CAPACITY_RESPONSE), (PUCHAR)Response);
+    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_READ_CAPACITY_CMD_LEN,
(PUCHAR)&Cmd, sizeof(UFI_CAPACITY_RESPONSE), (PUCHAR)Response, RetryCount);
 }
 
 NTSTATUS
 USBSTOR_SendModeSenseCmd(
     IN PDEVICE_OBJECT DeviceObject,
-    IN PIRP Irp)
+    IN PIRP Irp,
+    IN ULONG RetryCount)
 {
 #if 0
     UFI_SENSE_CMD Cmd;
@@ -1098,12 +1215,13 @@
 NTSTATUS
 USBSTOR_SendReadWriteCmd(
     IN PDEVICE_OBJECT DeviceObject,
-    IN PIRP Irp)
+    IN PIRP Irp,
+    IN ULONG RetryCount)
 {
     UFI_READ_WRITE_CMD Cmd;
     PPDO_DEVICE_EXTENSION PDODeviceExtension;
     PCDB pCDB;
-    ULONG BlockCount;
+    ULONG BlockCount, Temp;
     PIO_STACK_LOCATION IoStack;
     PSCSI_REQUEST_BLOCK Request;
 
@@ -1155,18 +1273,25 @@
     Cmd.LogicalBlockByte2 = pCDB->CDB10.LogicalBlockByte2;
     Cmd.LogicalBlockByte3 = pCDB->CDB10.LogicalBlockByte3;
 
+    //
+    // sanity check
+    //
+    Temp = (Cmd.ContiguousLogicBlocksByte0 << 8 | Cmd.ContiguousLogicBlocksByte1);
+    ASSERT(NTOHL(Temp == BlockCount));
+
     DPRINT("USBSTOR_SendReadWriteCmd BlockAddress %x%x%x%x BlockCount %lu
BlockLength %lu\n", Cmd.LogicalBlockByte0, Cmd.LogicalBlockByte1,
Cmd.LogicalBlockByte2, Cmd.LogicalBlockByte3, BlockCount,
PDODeviceExtension->BlockLength);
 
     //
     // send request
     //
-    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_READ_WRITE_CMD_LEN,
(PUCHAR)&Cmd, Request->DataTransferLength, (PUCHAR)Request->DataBuffer);
+    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_READ_WRITE_CMD_LEN,
(PUCHAR)&Cmd, Request->DataTransferLength, (PUCHAR)Request->DataBuffer,
RetryCount);
 }
 
 NTSTATUS
 USBSTOR_SendTestUnitCmd(
     IN PDEVICE_OBJECT DeviceObject,
-    IN OUT PIRP Irp)
+    IN OUT PIRP Irp,
+    IN ULONG RetryCount)
 {
     UFI_TEST_UNIT_CMD Cmd;
     PPDO_DEVICE_EXTENSION PDODeviceExtension;
@@ -1203,14 +1328,15 @@
     //
     // send the request
     //
-    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_TEST_UNIT_CMD_LEN,
(PUCHAR)&Cmd, 0, NULL);
+    return USBSTOR_SendRequest(DeviceObject, Irp, NULL, UFI_TEST_UNIT_CMD_LEN,
(PUCHAR)&Cmd, 0, NULL, RetryCount);
 }
 
 
 NTSTATUS
 USBSTOR_HandleExecuteSCSI(
     IN PDEVICE_OBJECT DeviceObject,
-    IN PIRP Irp)
+    IN PIRP Irp,
+    IN ULONG RetryCount)
 {
     PCDB pCDB;
     NTSTATUS Status;
@@ -1253,7 +1379,7 @@
         ASSERT(Request->DataBuffer);
 
         DPRINT("SCSIOP_READ_CAPACITY Length %lu\n",
Request->DataTransferLength);
-        Status = USBSTOR_SendCapacityCmd(DeviceObject, Irp);
+        Status = USBSTOR_SendCapacityCmd(DeviceObject, Irp, RetryCount);
     }
     else if (pCDB->MODE_SENSE.OperationCode == SCSIOP_MODE_SENSE)
     {
@@ -1264,7 +1390,7 @@
         //
         // send mode sense command
         //
-        Status = USBSTOR_SendModeSenseCmd(DeviceObject, Irp);
+        Status = USBSTOR_SendModeSenseCmd(DeviceObject, Irp, RetryCount);
     }
     else if (pCDB->MODE_SENSE.OperationCode == SCSIOP_READ || 
pCDB->MODE_SENSE.OperationCode == SCSIOP_WRITE)
     {
@@ -1273,7 +1399,7 @@
         //
         // send read / write command
         //
-        Status = USBSTOR_SendReadWriteCmd(DeviceObject, Irp);
+        Status = USBSTOR_SendReadWriteCmd(DeviceObject, Irp, RetryCount);
     }
     else if (pCDB->AsByte[0] == SCSIOP_MEDIUM_REMOVAL)
     {
@@ -1302,7 +1428,7 @@
         //
         // send test unit command
         //
-        Status = USBSTOR_SendTestUnitCmd(DeviceObject, Irp);
+        Status = USBSTOR_SendTestUnitCmd(DeviceObject, Irp, RetryCount);
     }
     else
     {

Modified: trunk/reactos/drivers/usb/usbstor/usbstor.c
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/usbsto…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/usbstor.c [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/usbstor.c [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -60,6 +60,11 @@
     DeviceExtension->LowerDeviceObject = IoAttachDeviceToDeviceStack(DeviceObject,
PhysicalDeviceObject);
 
     //
+    // init timer
+    //
+    IoInitializeTimer(DeviceObject, USBSTOR_TimerRoutine, (PVOID)DeviceExtension);
+
+    //
     // did attaching fail
     //
     if (!DeviceExtension->LowerDeviceObject)

Modified: trunk/reactos/drivers/usb/usbstor/usbstor.h
URL:
http://svn.reactos.org/svn/reactos/trunk/reactos/drivers/usb/usbstor/usbsto…
==============================================================================
--- trunk/reactos/drivers/usb/usbstor/usbstor.h [iso-8859-1] (original)
+++ trunk/reactos/drivers/usb/usbstor/usbstor.h [iso-8859-1] Fri Feb 17 03:02:14 2012
@@ -68,6 +68,9 @@
     ULONG IrpPendingCount;                                                              
// count of irp pending
     PSCSI_REQUEST_BLOCK ActiveSrb;                                                      
// stores the current active SRB
     KEVENT NoPendingRequests;                                                           
// set if no pending or in progress requests
+    PSCSI_REQUEST_BLOCK LastTimerActiveSrb;                                             
// last timer tick active srb
+    ULONG SrbErrorHandlingActive;                                                       
// error handling of srb is activated
+    ULONG TimerWorkQueueEnabled;                                                        
// timer work queue enabled
 }FDO_DEVICE_EXTENSION, *PFDO_DEVICE_EXTENSION;
 
 typedef struct
@@ -76,10 +79,14 @@
     PDEVICE_OBJECT LowerDeviceObject;                                                   
// points to FDO
     UCHAR LUN;                                                                          
// lun id
     PVOID InquiryData;                                                                  
// USB SCSI inquiry data
+    PUCHAR FormatData;                                                                  
// USB SCSI Read Format Capacity Data
     UCHAR Claimed;                                                                      
// indicating if it has been claimed by upper driver
     ULONG BlockLength;                                                                  
// length of block
     ULONG LastLogicBlockAddress;                                                        
// last block address
     PDEVICE_OBJECT *PDODeviceObject;                                                    
// entry in pdo list
+    PDEVICE_OBJECT Self;                                                                
// self
+    UCHAR MediumTypeCode;                                                               
// floppy medium type code
+    UCHAR IsFloppy;                                                                     
// is device floppy
 }PDO_DEVICE_EXTENSION, *PPDO_DEVICE_EXTENSION;
 
 //
@@ -247,6 +254,51 @@
     UCHAR Reserved[3];
 }UFI_TIMER_PROTECT_PAGE, *PUFI_TIMER_PROTECT_PAGE;
 C_ASSERT(sizeof(UFI_TIMER_PROTECT_PAGE) == 8);
+
+//--------------------------------------------------------------------------------------------------------------------------------------------
+//
+// UFI read capacity cmd
+//
+
+typedef struct
+{
+    UCHAR Code;
+    UCHAR LUN;
+    UCHAR Reserved[5];
+    UCHAR AllocationLengthMsb;
+    UCHAR AllocationLengthLsb;
+    UCHAR Reserved1[3];
+}UFI_READ_FORMAT_CAPACITY, *PUFI_READ_FORMAT_CAPACITY;
+
+C_ASSERT(sizeof(UFI_READ_FORMAT_CAPACITY) == 12);
+
+#define UFI_READ_FORMAT_CAPACITY_CMD_LEN (10)
+
+typedef struct
+{
+    UCHAR Reserved1;
+    UCHAR Reserved2;
+    UCHAR Reserved3;
+    UCHAR CapacityLength;
+}UFI_CAPACITY_FORMAT_HEADER, *PUFI_CAPACITY_FORMAT_HEADER;
+
+C_ASSERT(sizeof(UFI_CAPACITY_FORMAT_HEADER) == 4);
+
+typedef struct
+{
+    ULONG BlockCount;
+    UCHAR Code;
+    UCHAR BlockLengthByte0;
+    UCHAR BlockLengthByte1;
+    UCHAR BlockLengthByte2;
+}UFI_CAPACITY_DESCRIPTOR, *PUFI_CAPACITY_DESCRIPTOR;
+
+#define UNFORMATED_MEDIA_CODE_DESCRIPTORY_TYPE (1)
+#define FORMAT_MEDIA_CODE_DESCRIPTOR_TYPE      (2)
+#define CARTRIDGE_MEDIA_CODE_DESCRIPTOR_TYPE   (3)
+
+
+
 

//--------------------------------------------------------------------------------------------------------------------------------------------
 //
@@ -280,6 +332,7 @@
     PMDL TransferBufferMDL;
     PKEVENT Event;
     ULONG ErrorIndex;
+    ULONG RetryCount;
 }IRP_CONTEXT, *PIRP_CONTEXT;
 
 typedef struct _ERRORHANDLER_WORKITEM_DATA
@@ -361,6 +414,12 @@
     IN PDEVICE_OBJECT DeviceObject,
     IN PFDO_DEVICE_EXTENSION DeviceExtension);
 
+BOOLEAN
+USBSTOR_IsFloppy(
+    IN PUCHAR Buffer,
+    IN ULONG BufferLength,
+    OUT PUCHAR MediumTypeCode);
+
 //---------------------------------------------------------------------
 //
 // descriptor.c routines
@@ -386,11 +445,20 @@
 NTSTATUS
 USBSTOR_HandleExecuteSCSI(
     IN PDEVICE_OBJECT DeviceObject,
-    IN PIRP Irp);
+    IN PIRP Irp,
+    IN ULONG RetryCount);
 
 NTSTATUS
 USBSTOR_SendInquiryCmd(
-    IN PDEVICE_OBJECT DeviceObject);
+    IN PDEVICE_OBJECT DeviceObject,
+    IN ULONG RetryCount);
+
+NTSTATUS
+USBSTOR_SendFormatCapacity(
+    IN PDEVICE_OBJECT DeviceObject,
+    IN ULONG RetryCount);
+
+
 
 NTSTATUS
 NTAPI
@@ -399,6 +467,17 @@
     PIRP Irp, 
     PVOID Ctx);
 
+NTSTATUS
+USBSTOR_SendCBW(
+    PIRP_CONTEXT Context,
+    PIRP Irp);
+
+VOID
+USBSTOR_SendCSW(
+    PIRP_CONTEXT Context,
+    PIRP Irp);
+
+
 //---------------------------------------------------------------------
 //
 // disk.c routines
@@ -479,3 +558,9 @@
     IN PDEVICE_OBJECT DeviceObject,
     IN USBD_PIPE_HANDLE PipeHandle);
 
+VOID
+NTAPI
+USBSTOR_TimerRoutine(
+    PDEVICE_OBJECT DeviceObject,
+     PVOID Context);
+