Subversion Repositories Kolibri OS

Rev

Rev 4549 | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. ; standard driver stuff; version of driver model = 5
  2. format PE DLL native 0.05
  3. entry START
  4.  
  5. DEBUG = 1
  6. DUMP_PACKETS = 0
  7.  
  8. ; this is for DEBUGF macro from 'fdo.inc'
  9. __DEBUG__ = 1
  10. __DEBUG_LEVEL__ = 1
  11.  
  12. include '../struct.inc'
  13.  
  14. ; USB constants
  15. DEVICE_DESCR_TYPE = 1
  16. CONFIG_DESCR_TYPE = 2
  17. STRING_DESCR_TYPE = 3
  18. INTERFACE_DESCR_TYPE = 4
  19. ENDPOINT_DESCR_TYPE = 5
  20. DEVICE_QUALIFIER_DESCR_TYPE = 6
  21.  
  22. CONTROL_PIPE = 0
  23. ISOCHRONOUS_PIPE = 1
  24. BULK_PIPE = 2
  25. INTERRUPT_PIPE = 3
  26.  
  27. ; USB structures
  28. struct config_descr
  29. bLength                 db      ?
  30. bDescriptorType         db      ?
  31. wTotalLength            dw      ?
  32. bNumInterfaces          db      ?
  33. bConfigurationValue     db      ?
  34. iConfiguration          db      ?
  35. bmAttributes            db      ?
  36. bMaxPower               db      ?
  37. ends
  38.  
  39. struct interface_descr
  40. bLength                 db      ?
  41. bDescriptorType         db      ?
  42. bInterfaceNumber        db      ?
  43. bAlternateSetting       db      ?
  44. bNumEndpoints           db      ?
  45. bInterfaceClass         db      ?
  46. bInterfaceSubClass      db      ?
  47. bInterfaceProtocol      db      ?
  48. iInterface              db      ?
  49. ends
  50.  
  51. struct endpoint_descr
  52. bLength                 db      ?
  53. bDescriptorType         db      ?
  54. bEndpointAddress        db      ?
  55. bmAttributes            db      ?
  56. wMaxPacketSize          dw      ?
  57. bInterval               db      ?
  58. ends
  59.  
  60. ; Mass storage protocol constants, USB layer
  61. REQUEST_GETMAXLUN = 0xFE        ; get max lun
  62. REQUEST_BORESET = 0xFF          ; bulk-only reset
  63.  
  64. ; Mass storage protocol structures, USB layer
  65. ; Sent from host to device in the first stage of an operation.
  66. struct command_block_wrapper
  67. Signature       dd      ?       ; the constant 'USBC'
  68. Tag             dd      ?       ; identifies response with request
  69. Length          dd      ?       ; length of data-transport phase
  70. Flags           db      ?       ; one of CBW_FLAG_*
  71. CBW_FLAG_OUT = 0
  72. CBW_FLAG_IN = 80h
  73. LUN             db      ?       ; addressed unit
  74. CommandLength   db      ?       ; the length of the following field
  75. Command         rb      16
  76. ends
  77.  
  78. ; Sent from device to host in the last stage of an operation.
  79. struct command_status_wrapper
  80. Signature       dd      ?       ; the constant 'USBS'
  81. Tag             dd      ?       ; identifies response with request
  82. LengthRest      dd      ?       ; .Length - (size of data which were transferred)
  83. Status          db      ?       ; one of CSW_STATUS_*
  84. CSW_STATUS_OK = 0
  85. CSW_STATUS_FAIL = 1
  86. CSW_STATUS_FATAL = 2
  87. ends
  88.  
  89. ; Constants of SCSI layer
  90. SCSI_REQUEST_SENSE = 3
  91. SCSI_INQUIRY = 12h
  92. SCSI_READ_CAPACITY = 25h
  93. SCSI_READ10 = 28h
  94. SCSI_WRITE10 = 2Ah
  95.  
  96. ; Result of SCSI REQUEST SENSE command.
  97. SENSE_UNKNOWN = 0
  98. SENSE_RECOVERED_ERROR = 1
  99. SENSE_NOT_READY = 2
  100. SENSE_MEDIUM_ERROR = 3
  101. SENSE_HARDWARE_ERROR = 4
  102. SENSE_ILLEGAL_REQUEST = 5
  103. SENSE_UNIT_ATTENTION = 6
  104. SENSE_DATA_PROTECT = 7
  105. SENSE_BLANK_CHECK = 8
  106. ; 9 is vendor-specific
  107. SENSE_COPY_ABORTED = 10
  108. SENSE_ABORTED_COMMAND = 11
  109. SENSE_EQUAL = 12
  110. SENSE_VOLUME_OVERFLOW = 13
  111. SENSE_MISCOMPARE = 14
  112. ; 15 is reserved
  113.  
  114. ; Structures of SCSI layer
  115. ; Result of SCSI INQUIRY request.
  116. struct inquiry_data
  117. PeripheralDevice        db      ?       ; lower 5 bits are PeripheralDeviceType
  118.                                         ; upper 3 bits are PeripheralQualifier
  119. RemovableMedium         db      ?       ; upper bit is RemovableMedium
  120.                                         ; other bits are for compatibility
  121. Version                 db      ?       ; lower 3 bits are ANSI-Approved version
  122.                                         ; next 3 bits are ECMA version
  123.                                         ; upper 2 bits are ISO version
  124. ResponseDataFormat      db      ?       ; lower 4 bits are ResponseDataFormat
  125.                                         ; bit 6 is TrmIOP
  126.                                         ; bit 7 is AENC
  127. AdditionalLength        db      ?
  128.                         dw      ?       ; reserved
  129. Flags                   db      ?
  130. VendorID                rb      8       ; vendor ID, big-endian
  131. ProductID               rb      16      ; product ID, big-endian
  132. ProductRevBE            dd      ?       ; product revision, big-endian
  133. ends
  134.  
  135. struct sense_data
  136. ErrorCode               db      ?       ; lower 7 bits are error code:
  137.                                         ; 70h = current error,
  138.                                         ; 71h = deferred error
  139.                                         ; upper bit is InformationValid
  140. SegmentNumber           db      ?       ; number of segment descriptor
  141.                                         ; for commands COPY [+VERIFY], COMPARE
  142. SenseKey                db      ?       ; bits 0-3 are one of SENSE_*
  143.                                         ; bit 4 is reserved
  144.                                         ; bit 5 is IncorrectLengthIndicator
  145.                                         ; bits 6 and 7 are used by
  146.                                         ; sequential-access devices
  147. Information             dd      ?       ; command-specific
  148. AdditionalLength        db      ?       ; length of data starting here
  149. CommandInformation      dd      ?       ; command-specific
  150. AdditionalSenseCode     db      ?       ; \ more detailed error code
  151. AdditionalSenseQual     db      ?       ; / standard has a large table of them
  152. FRUCode                 db      ?       ; which part of device has failed
  153.                                         ; (device-specific, not regulated)
  154. SenseKeySpecific        rb      3       ; depends on SenseKey
  155. ends
  156.  
  157. ; Device data
  158. ; USB Mass storage device has one or more logical units, identified by LUN,
  159. ; logical unit number. The highest value of LUN, that is, number of units
  160. ; minus 1, can be obtained via control request Get Max LUN.
  161. struct usb_device_data
  162. ConfigPipe              dd      ?       ; configuration pipe
  163. OutPipe                 dd      ?       ; pipe for OUT bulk endpoint
  164. InPipe                  dd      ?       ; pipe for IN bulk endpoint
  165. MaxLUN                  dd      ?       ; maximum Logical Unit Number
  166. LogicalDevices          dd      ?       ; pointer to array of usb_unit_data
  167. ; 1 for a connected USB device, 1 for each disk device
  168. ; the structure can be freed when .NumReferences decreases to zero
  169. NumReferences           dd      ?       ; number of references
  170. ConfigRequest           rb      8       ; buffer for configuration requests
  171. LengthRest              dd      ?       ; Length - (size of data which were transferred)
  172. ; All requests to a given device are serialized,
  173. ; only one request to a given device can be processed at a time.
  174. ; The current request and all pending requests are organized in the following
  175. ; queue, the head being the current request.
  176. ; NB: the queue must be device-wide due to the protocol:
  177. ; data stage is not tagged (unlike command_*_wrapper), so the only way to know
  178. ; what request the data are associated with is to guarantee that only one
  179. ; request is processing at the time.
  180. RequestsQueue           rd      2
  181. QueueLock               rd      3       ; protects .RequestsQueue
  182. InquiryData             inquiry_data    ; information about device
  183. ; data for the current request
  184. Command                 command_block_wrapper
  185. DeviceDisconnected      db      ?
  186. Status                  command_status_wrapper
  187. Sense                   sense_data
  188. ends
  189.  
  190. ; Information about one logical device.
  191. struct usb_unit_data
  192. Parent          dd      ?       ; pointer to parent usb_device_data
  193. LUN             db      ?       ; index in usb_device_data.LogicalDevices array
  194. DiskIndex       db      ?       ; for name "usbhd<index>"
  195. MediaPresent    db      ?
  196.                 db      ?       ; alignment
  197. DiskDevice      dd      ?       ; handle of disk device or NULL
  198. SectorSize      dd      ?       ; sector size
  199. ; For some devices, the first request to the medium fails with 'unit not ready'.
  200. ; When the code sees this status, it retries the command several times.
  201. ; Two following variables track the retry count and total time for those;
  202. ; total time is currently used only for debug output.
  203. UnitReadyAttempts       dd      ?
  204. TimerTicks              dd      ?
  205. ends
  206.  
  207. ; This is the structure for items in the queue usb_device_data.RequestsQueue.
  208. struct request_queue_item
  209. Next            dd      ?       ; next item in the queue
  210. Prev            dd      ?       ; prev item in the queue
  211. ReqBuilder      dd      ?       ; procedure to fill command_block_wrapper
  212. Buffer          dd      ?       ; input or output data
  213.                                 ; (length is command_block_wrapper.Length)
  214. Callback        dd      ?       ; procedure to call in the end of transfer
  215. UserData        dd      ?       ; passed as-is to .Callback
  216. ; There are 3 possible stages of any request, one of them optional:
  217. ; command stage (host sends command_block_wrapper to device),
  218. ; optional data stage,
  219. ; status stage (device sends command_status_wrapper to host).
  220. ; Also, if a request fails, the code queues additional request
  221. ; SCSI_REQUEST_SENSE; sense_data from SCSI_REQUEST_SENSE
  222. ; contains some information about the error.
  223. Stage           db      ?
  224. ends
  225.  
  226. section '.flat' code readable writable executable
  227. include '../proc32.inc'
  228. include '../peimport.inc'
  229. include '../fdo.inc'
  230. include '../macros.inc'
  231.  
  232. ; The start procedure.
  233. proc START
  234. virtual at esp
  235.         dd      ?       ; return address
  236. .reason dd      ?       ; DRV_ENTRY or DRV_EXIT
  237. .cmdline dd     ?
  238. end virtual
  239. ; 1. Test whether the procedure is called with the argument DRV_ENTRY.
  240. ; If not, return 0.
  241.         xor     eax, eax        ; initialize return value
  242.         cmp     [.reason], 1    ; compare the argument
  243.         jnz     .nothing
  244. ; 2. Initialize: we have one global mutex.
  245.         mov     ecx, free_numbers_lock
  246.         invoke  MutexInit
  247. ; 3. Register self as a USB driver.
  248. ; The name is my_driver = 'usbstor'; IOCTL interface is not supported;
  249. ; usb_functions is an offset of a structure with callback functions.
  250.         invoke  RegUSBDriver, my_driver, 0, usb_functions
  251. ; 4. Return the returned value of RegUSBDriver.
  252. .nothing:
  253.         ret
  254. endp
  255.  
  256. ; Helper procedures to work with requests queue.
  257.  
  258. ; Add a request to the queue. Stdcall with 5 arguments.
  259. proc queue_request
  260.         push    ebx esi
  261. virtual at esp
  262.                 rd      2       ; saved registers
  263.                 dd      ?       ; return address
  264. .device         dd      ?       ; pointer to usb_device_data
  265. .ReqBuilder     dd      ?       ; request_queue_item.ReqBuilder
  266. .Buffer         dd      ?       ; request_queue_item.Buffer
  267. .Callback       dd      ?       ; request_queue_item.Callback
  268. .UserData       dd      ?       ; request_queue_item.UserData
  269. end virtual
  270. ; 1. Allocate the memory for the request description.
  271.         movi    eax, sizeof.request_queue_item
  272.         invoke  Kmalloc
  273.         test    eax, eax
  274.         jnz     @f
  275.         mov     esi, nomemory
  276.         invoke  SysMsgBoardStr
  277.         pop     esi ebx
  278.         ret     20
  279. @@:
  280. ; 2. Fill user-provided parts of the request description.
  281.         push    edi
  282.         xchg    eax, ebx
  283.         lea     esi, [.ReqBuilder+4]
  284.         lea     edi, [ebx+request_queue_item.ReqBuilder]
  285.         movsd   ; ReqBuilder
  286.         movsd   ; Buffer
  287.         movsd   ; Callback
  288.         movsd   ; UserData
  289.         pop     edi
  290. ; 3. Set stage to zero: not started.
  291.         mov     [ebx+request_queue_item.Stage], 0
  292. ; 4. Lock the queue.
  293.         mov     esi, [.device]
  294.         lea     ecx, [esi+usb_device_data.QueueLock]
  295.         invoke  MutexLock
  296. ; 5. Insert the request to the tail of the queue.
  297.         add     esi, usb_device_data.RequestsQueue
  298.         mov     edx, [esi+request_queue_item.Prev]
  299.         mov     [ebx+request_queue_item.Next], esi
  300.         mov     [ebx+request_queue_item.Prev], edx
  301.         mov     [edx+request_queue_item.Next], ebx
  302.         mov     [esi+request_queue_item.Prev], ebx
  303. ; 6. Test whether the queue was empty
  304. ; and the request should be started immediately.
  305.         cmp     [esi+request_queue_item.Next], ebx
  306.         jnz     .unlock
  307. ; 8. If the step 6 shows that the request is the first in the queue,
  308. ; start it.
  309.         sub     esi, usb_device_data.RequestsQueue
  310.         call    setup_request
  311.         jmp     .nothing
  312. .unlock:
  313.         invoke  MutexUnlock
  314. ; 9. Return.
  315. .nothing:
  316.         pop     esi ebx
  317.         ret     20
  318. endp
  319.  
  320. ; The current request is completed. Call the callback,
  321. ; remove the request from the queue, start the next
  322. ; request if there is one.
  323. ; esi points to usb_device_data
  324. proc complete_request
  325. ; 1. Print common debug messages on fails.
  326. if DEBUG
  327.         cmp     [esi+usb_device_data.Status.Status], CSW_STATUS_FAIL
  328.         jb      .normal
  329.         jz      .fail
  330.         DEBUGF 1, 'K : Fatal error during execution of command %x\n', [esi+usb_device_data.Command.Command]:2
  331.         jmp     .normal
  332. .fail:
  333.         DEBUGF 1, 'K : Command %x failed\n', [esi+usb_device_data.Command.Command]:2
  334. .normal:
  335. end if
  336. ; 2. Get the current request.
  337.         mov     ebx, [esi+usb_device_data.RequestsQueue+request_queue_item.Next]
  338. ; 3. Call the callback.
  339.         stdcall [ebx+request_queue_item.Callback], esi, [ebx+request_queue_item.UserData]
  340. ; 4. Lock the queue.
  341.         lea     ecx, [esi+usb_device_data.QueueLock]
  342.         invoke  MutexLock
  343. ; 5. Remove the request.
  344.         lea     edx, [esi+usb_device_data.RequestsQueue]
  345.         mov     eax, [ebx+request_queue_item.Next]
  346.         mov     [eax+request_queue_item.Prev], edx
  347.         mov     [edx+request_queue_item.Next], eax
  348. ; 6. Free the request memory.
  349.         push    eax edx
  350.         xchg    eax, ebx
  351.         invoke  Kfree
  352.         pop     edx ebx
  353. ; 7. If there is a next request, start processing.
  354.         cmp     ebx, edx
  355.         jnz     setup_request
  356. ; 8. Unlock the queue and return.
  357.         lea     ecx, [esi+usb_device_data.QueueLock]
  358.         invoke  MutexUnlock
  359.         ret
  360. endp
  361.  
  362. ; Start processing the request. Called either by queue_request
  363. ; or when the previous request has been processed.
  364. ; Do not call directly, use queue_request.
  365. ; Must be called when queue is locked; unlocks the queue when returns.
  366. proc setup_request
  367.         xor     eax, eax
  368. ; 1. If DeviceDisconnected has been run, then all handles of pipes
  369. ; are invalid, so we must fail immediately.
  370. ; (That is why this function needs the locked queue: this
  371. ; guarantee that either DeviceDisconnected has been already run, or
  372. ; DeviceDisconnected will not return before the queue is unlocked.)
  373.         cmp     [esi+usb_device_data.DeviceDisconnected], al
  374.         jnz     .fatal
  375. ; 2. If the previous command has encountered a fatal error,
  376. ; perform reset recovery.
  377.         cmp     [esi+usb_device_data.Status.Status], CSW_STATUS_FATAL
  378.         jb      .norecovery
  379. ; 2a. Send Bulk-Only Mass Storage Reset command to config pipe.
  380.         lea     edx, [esi+usb_device_data.ConfigRequest]
  381.         mov     word [edx], (REQUEST_BORESET shl 8) + 21h       ; class request
  382.         mov     word [edx+6], ax        ; length = 0
  383.         invoke  USBControlTransferAsync, [esi+usb_device_data.ConfigPipe], edx, eax, eax, recovery_callback1, esi, eax
  384. ; 2b. Fail here = fatal error.
  385.         test    eax, eax
  386.         jz      .fatal
  387. ; 2c. Otherwise, unlock the queue and return. recovery_callback1 will continue processing.
  388. .unlock_return:
  389.         lea     ecx, [esi+usb_device_data.QueueLock]
  390.         invoke  MutexUnlock
  391.         ret
  392. .norecovery:
  393. ; 3. Send the command. Fail (no memory or device disconnected) = fatal error.
  394. ; Otherwise, go to 2c.
  395.         call    request_stage1
  396.         test    eax, eax
  397.         jnz     .unlock_return
  398. .fatal:
  399. ; 4. Fatal error. Set status = FATAL, unlock the queue, complete the request.
  400.         mov     [esi+usb_device_data.Status.Status], CSW_STATUS_FATAL
  401.         lea     ecx, [esi+usb_device_data.QueueLock]
  402.         invoke  MutexUnlock
  403.         jmp     complete_request
  404. endp
  405.  
  406. ; Initiate USB transfer for the first stage of a request (send command).
  407. proc request_stage1
  408.         mov     ebx, [esi+usb_device_data.RequestsQueue+request_queue_item.Next]
  409. ; 1. Set the stage to 1 = command stage.
  410.         inc     [ebx+request_queue_item.Stage]
  411. ; 2. Generate the command. Zero-initialize and use the caller-provided proc.
  412.         lea     edx, [esi+usb_device_data.Command]
  413.         xor     eax, eax
  414.         mov     [edx+command_block_wrapper.CommandLength], 12
  415.         mov     dword [edx+command_block_wrapper.Command], eax
  416.         mov     dword [edx+command_block_wrapper.Command+4], eax
  417.         mov     dword [edx+command_block_wrapper.Command+8], eax
  418.         mov     dword [edx+command_block_wrapper.Command+12], eax
  419.         inc     [edx+command_block_wrapper.Tag]
  420.         stdcall [ebx+request_queue_item.ReqBuilder], edx, [ebx+request_queue_item.UserData]
  421. ; 4. Initiate USB transfer.
  422.         lea     edx, [esi+usb_device_data.Command]
  423. if DUMP_PACKETS
  424.         DEBUGF 1,'K : USBSTOR out:'
  425.         mov     eax, edx
  426.         mov     ecx, sizeof.command_block_wrapper
  427.         call    debug_dump
  428.         DEBUGF 1,'\n'
  429. end if
  430.         invoke  USBNormalTransferAsync, [esi+usb_device_data.OutPipe], edx, sizeof.command_block_wrapper, request_callback1, esi, 0
  431.         test    eax, eax
  432.         jz      .nothing
  433. ; 5. If the next stage is data stage in the same direction, enqueue it here.
  434.         cmp     [esi+usb_device_data.Command.Flags], 0
  435.         js      .nothing
  436.         cmp     [esi+usb_device_data.Command.Length], 0
  437.         jz      .nothing
  438.         mov     edx, [esi+usb_device_data.RequestsQueue+request_queue_item.Next]
  439. if DUMP_PACKETS
  440.         DEBUGF 1,'K : USBSTOR out:'
  441.         mov     eax, [edx+request_queue_item.Buffer]
  442.         mov     ecx, [esi+usb_device_data.Command.Length]
  443.         call    debug_dump
  444.         DEBUGF 1,'\n'
  445. end if
  446.         invoke  USBNormalTransferAsync, [esi+usb_device_data.OutPipe], [edx+request_queue_item.Buffer], [esi+usb_device_data.Command.Length], request_callback2, esi, 0
  447. .nothing:
  448.         ret
  449. endp
  450.  
  451. if DUMP_PACKETS
  452. proc debug_dump
  453.         test    ecx, ecx
  454.         jz      .done
  455. .loop:
  456.         test    ecx, 0Fh
  457.         jnz     @f
  458.         DEBUGF 1,'\nK :'
  459. @@:
  460.         DEBUGF 1,' %x',[eax]:2
  461.         inc     eax
  462.         dec     ecx
  463.         jnz     .loop
  464. .done:
  465.         ret
  466. endp
  467. end if
  468.  
  469. ; Called when the Reset command is completed,
  470. ; either successfully or not.
  471. proc recovery_callback1
  472. virtual at esp
  473.                 dd      ?       ; return address
  474. .pipe           dd      ?
  475. .status         dd      ?
  476. .buffer         dd      ?
  477. .length         dd      ?
  478. .calldata       dd      ?
  479. end virtual
  480.         cmp     [.status], 0
  481.         jnz     .error
  482. ; todo: reset pipes
  483.         push    ebx esi
  484.         mov     esi, [.calldata+8]
  485.         call    request_stage1
  486.         pop     esi ebx
  487.         test    eax, eax
  488.         jz      .error
  489.         ret     20
  490. .error:
  491.         DEBUGF 1, 'K : error %d while resetting', [.status+24h]
  492.         jmp     request_callback1.common_error
  493. endp
  494.  
  495. ; Called when the first stage of request is completed,
  496. ; either successfully or not.
  497. proc request_callback1
  498. virtual at esp
  499.                 dd      ?       ; return address
  500. .pipe           dd      ?
  501. .status         dd      ?
  502. .buffer         dd      ?
  503. .length         dd      ?
  504. .calldata       dd      ?
  505. end virtual
  506. ; 1. Initialize.
  507.         mov     ecx, [.calldata]
  508.         mov     eax, [.status]
  509. ; 2. Test for error.
  510.         test    eax, eax
  511.         jnz     .error
  512. ; No error.
  513. ; 3. Increment the stage.
  514.         mov     edx, [ecx+usb_device_data.RequestsQueue+request_queue_item.Next]
  515.         inc     [edx+request_queue_item.Stage]
  516. ; 4. Check whether we need to send the data.
  517. ; 4a. If there is no data, skip this stage.
  518.         cmp     [ecx+usb_device_data.Command.Length], 0
  519.         jz      ..request_get_status
  520. ; 4b. If data were enqueued in the first stage, do nothing, wait for request_callback2.
  521.         cmp     [ecx+usb_device_data.Command.Flags], 0
  522.         jns     .nothing
  523. ; 5. Initiate USB transfer. If this fails, go to the error handler.
  524.         invoke  USBNormalTransferAsync, [ecx+usb_device_data.InPipe], [edx+request_queue_item.Buffer], [ecx+usb_device_data.Command.Length], request_callback2, ecx, 0
  525.         test    eax, eax
  526.         jz      .error
  527. ; 6. The status stage goes to the same direction, enqueue it now.
  528.         mov     ecx, [.calldata]
  529.         jmp     ..enqueue_status
  530. .nothing:
  531.         ret     20
  532. .error:
  533. ; Error.
  534. ; 7. Print debug message and complete the request as failed.
  535.         DEBUGF 1,'K : error %d after %d bytes in request stage\n',eax,[.length+24h]
  536. ; If device is disconnected and data stage is enqueued, do nothing;
  537. ; data stage callback will do everything.
  538.         cmp     eax, 16
  539.         jnz     .common_error
  540.         cmp     [ecx+usb_device_data.Command.Flags], 0
  541.         js      .common_error
  542.         cmp     [ecx+usb_device_data.Command.Length], 0
  543.         jz      .common_error
  544.         ret     20
  545. .common_error:
  546. ; TODO: add recovery after STALL
  547.         mov     ecx, [.calldata]
  548.         mov     [ecx+usb_device_data.Status.Status], CSW_STATUS_FATAL
  549.         push    ebx esi
  550.         mov     esi, ecx
  551.         call    complete_request
  552.         pop     esi ebx
  553.         ret     20
  554. endp
  555.  
  556. ; Called when the second stage of request is completed,
  557. ; either successfully or not.
  558. proc request_callback2
  559. virtual at esp
  560.                 dd      ?       ; return address
  561. .pipe           dd      ?
  562. .status         dd      ?
  563. .buffer         dd      ?
  564. .length         dd      ?
  565. .calldata       dd      ?
  566. end virtual
  567. if DUMP_PACKETS
  568.         mov     eax, [.calldata]
  569.         mov     eax, [eax+usb_device_data.InPipe]
  570.         cmp     [.pipe], eax
  571.         jnz     @f
  572.         DEBUGF 1,'K : USBSTOR in:'
  573.         push    eax ecx
  574.         mov     eax, [.buffer+8]
  575.         mov     ecx, [.length+8]
  576.         call    debug_dump
  577.         pop     ecx eax
  578.         DEBUGF 1,'\n'
  579. @@:
  580. end if
  581. ; 1. Initialize.
  582.         mov     ecx, [.calldata]
  583.         mov     eax, [.status]
  584. ; 2. Test for error.
  585.         test    eax, eax
  586.         jnz     .error
  587. ; No error.
  588. ; If the previous stage was in same direction, do nothing; status request is already enqueued.
  589.         cmp     [ecx+usb_device_data.Command.Flags], 0
  590.         js      .nothing
  591. ..request_get_status:
  592. ; 3. Increment the stage.
  593.         mov     edx, [ecx+usb_device_data.RequestsQueue+request_queue_item.Next]
  594.         inc     [edx+request_queue_item.Stage]
  595. ; 4. Initiate USB transfer. If this fails, go to the error handler.
  596. ..enqueue_status:
  597.         lea     edx, [ecx+usb_device_data.Status]
  598.         invoke  USBNormalTransferAsync, [ecx+usb_device_data.InPipe], edx, sizeof.command_status_wrapper, request_callback3, ecx, 0
  599.         test    eax, eax
  600.         jz      .error
  601. .nothing:
  602.         ret     20
  603. .error:
  604. ; Error.
  605. ; 5. Print debug message and complete the request as failed.
  606.         DEBUGF 1,'K : error %d after %d bytes in data stage\n',eax,[.length+24h]
  607. ; If device is disconnected and data stage is enqueued, do nothing;
  608. ; status stage callback will do everything.
  609.         cmp     [ecx+usb_device_data.Command.Flags], 0
  610.         js      .nothing
  611.         jmp     request_callback1.common_error
  612. endp
  613.  
  614. ; Called when the third stage of request is completed,
  615. ; either successfully or not.
  616. proc request_callback3
  617. virtual at esp
  618.                 dd      ?       ; return address
  619. .pipe           dd      ?
  620. .status         dd      ?
  621. .buffer         dd      ?
  622. .length         dd      ?
  623. .calldata       dd      ?
  624. end virtual
  625. if DUMP_PACKETS
  626.         DEBUGF 1,'K : USBSTOR in:'
  627.         mov     eax, [.buffer]
  628.         mov     ecx, [.length]
  629.         call    debug_dump
  630.         DEBUGF 1,'\n'
  631. end if
  632. ; 1. Initialize.
  633.         mov     eax, [.status]
  634. ; 2. Test for error.
  635.         test    eax, eax
  636.         jnz     .transfer_error
  637. ; Transfer is OK.
  638. ; 3. Validate the status. Invalid status = fatal error.
  639.         push    ebx esi
  640.         mov     esi, [.calldata+8]
  641.         mov     ebx, [esi+usb_device_data.RequestsQueue+request_queue_item.Next]
  642.         cmp     [esi+usb_device_data.Status.Signature], 'USBS'
  643.         jnz     .invalid
  644.         mov     eax, [esi+usb_device_data.Command.Tag]
  645.         cmp     [esi+usb_device_data.Status.Tag], eax
  646.         jnz     .invalid
  647.         cmp     [esi+usb_device_data.Status.Status], CSW_STATUS_FATAL
  648.         ja      .invalid
  649. ; 4. The status block is valid. Check the status code.
  650.         jz      .complete
  651. ; 5. If this command was not REQUEST_SENSE, copy status data to safe place.
  652. ; Otherwise, the original command has failed, so restore the fail status.
  653.         cmp     byte [esi+usb_device_data.Command.Command], SCSI_REQUEST_SENSE
  654.         jz      .request_sense
  655.         mov     eax, [esi+usb_device_data.Status.LengthRest]
  656.         mov     [esi+usb_device_data.LengthRest], eax
  657.         cmp     [esi+usb_device_data.Status.Status], CSW_STATUS_FAIL
  658.         jz      .fail
  659. .complete:
  660.         call    complete_request
  661. .nothing:
  662.         pop     esi ebx
  663.         ret     20
  664. .request_sense:
  665.         mov     [esi+usb_device_data.Status.Status], CSW_STATUS_FAIL
  666.         jmp     .complete
  667. .invalid:
  668. ; 6. Invalid status block. Say error, set status to fatal and complete request.
  669.         push    esi
  670.         mov     esi, invresponse
  671.         invoke  SysMsgBoardStr
  672.         pop     esi
  673.         mov     [esi+usb_device_data.Status.Status], CSW_STATUS_FATAL
  674.         jmp     .complete
  675. .fail:
  676. ; 7. The command has failed.
  677. ; If this command was not REQUEST_SENSE, schedule the REQUEST_SENSE command
  678. ; to determine the reason of fail. Otherwise, assume that there is no error data.
  679.         cmp     [esi+usb_device_data.Command.Command], SCSI_REQUEST_SENSE
  680.         jz      .fail_request_sense
  681.         mov     [ebx+request_queue_item.ReqBuilder], request_sense_req
  682.         lea     eax, [esi+usb_device_data.Sense]
  683.         mov     [ebx+request_queue_item.Buffer], eax
  684.         call    request_stage1
  685.         test    eax, eax
  686.         jnz     .nothing
  687. .fail_request_sense:
  688.         DEBUGF 1,'K : fail during REQUEST SENSE\n'
  689.         mov     byte [esi+usb_device_data.Sense], 0
  690.         jmp     .complete
  691. .transfer_error:
  692. ; TODO: add recovery after STALL
  693.         DEBUGF 1,'K : error %d after %d bytes in status stage\n',eax,[.length+24h]
  694.         jmp     request_callback1.common_error
  695. endp
  696.  
  697. ; Builder for SCSI_REQUEST_SENSE request.
  698. ; edx = first argument = pointer to usb_device_data.Command,
  699. ; second argument = custom data given to queue_request (ignored).
  700. proc request_sense_req
  701.         mov     [edx+command_block_wrapper.Length], sizeof.sense_data
  702.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_IN
  703.         mov     byte [edx+command_block_wrapper.Command+0], SCSI_REQUEST_SENSE
  704.         mov     byte [edx+command_block_wrapper.Command+4], sizeof.sense_data
  705.         ret     8
  706. endp
  707.  
  708. ; This procedure is called when new mass-storage device is detected.
  709. ; It initializes the device.
  710. ; Technically, initialization implies sending several USB queries,
  711. ; so it is split in several procedures. The first is AddDevice,
  712. ; other are callbacks which will be called at some time in the future,
  713. ; when the device will respond.
  714. ; The general scheme:
  715. ; * AddDevice parses descriptors, opens pipes; if everything is ok,
  716. ;   AddDevice sends REQUEST_GETMAXLUN with callback known_lun_callback;
  717. ; * known_lun_callback allocates memory for LogicalDevices and sends
  718. ;   SCSI_TEST_UNIT_READY to all logical devices with test_unit_ready_callback;
  719. ; * test_unit_ready_callback checks whether the unit is ready;
  720. ;   if not, it repeats the same request several times;
  721. ;   if ok or there were too many attempts, it sends SCSI_INQUIRY with
  722. ;   callback inquiry_callback;
  723. ; * inquiry_callback checks that a logical device is a block device
  724. ;   and the unit was ready; if so, it notifies the kernel about new disk device.
  725. proc AddDevice
  726.         push    ebx esi
  727. virtual at esp
  728.                 rd      2       ; saved registers ebx, esi
  729.                 dd      ?       ; return address
  730. .pipe0          dd      ?       ; handle of the config pipe
  731. .config         dd      ?       ; pointer to config_descr
  732. .interface      dd      ?       ; pointer to interface_descr
  733. end virtual
  734. ; 1. Check device type. Currently only SCSI-command-set Bulk-only devices
  735. ; are supported.
  736. ; 1a. Get the subclass and the protocol. Since bInterfaceSubClass and
  737. ; bInterfaceProtocol are subsequent in interface_descr, just one
  738. ; memory reference is used for both.
  739.         mov     esi, [.interface]
  740.         xor     ebx, ebx
  741.         mov     cx, word [esi+interface_descr.bInterfaceSubClass]
  742. ; 1b. For Mass-storage SCSI-command-set Bulk-only devices subclass must be 6
  743. ; and protocol must be 50h. Check.
  744.         cmp     cx, 0x5006
  745.         jz      .known
  746. ; There are devices with subclass 5 which use the same protocol 50h.
  747. ; The difference is not important for the code except for this test,
  748. ; so allow them to proceed also.
  749.         cmp     cx, 0x5005
  750.         jz      .known
  751. ; 1c. If the device is unknown, print a message and go to 11c.
  752.         mov     esi, unkdevice
  753.         invoke  SysMsgBoardStr
  754.         jmp     .nothing
  755. ; 1d. If the device uses known command set, print a message and continue
  756. ; configuring.
  757. .known:
  758.         push    esi
  759.         mov     esi, okdevice
  760.         invoke  SysMsgBoardStr
  761.         pop     esi
  762. ; 2. Allocate memory for internal device data.
  763. ; 2a. Call the kernel.
  764.         mov     eax, sizeof.usb_device_data
  765.         invoke  Kmalloc
  766. ; 2b. Check return value.
  767.         test    eax, eax
  768.         jnz     @f
  769. ; 2c. If failed, say a message and go to 11c.
  770.         mov     esi, nomemory
  771.         invoke  SysMsgBoardStr
  772.         jmp     .nothing
  773. @@:
  774. ; 2d. If succeeded, zero the contents and continue configuring.
  775.         xchg    ebx, eax        ; ebx will point to usb_device_data
  776.         xor     eax, eax
  777.         mov     [ebx+usb_device_data.OutPipe], eax
  778.         mov     [ebx+usb_device_data.InPipe], eax
  779.         mov     [ebx+usb_device_data.MaxLUN], eax
  780.         mov     [ebx+usb_device_data.LogicalDevices], eax
  781.         mov     dword [ebx+usb_device_data.ConfigRequest], eax
  782.         mov     dword [ebx+usb_device_data.ConfigRequest+4], eax
  783.         mov     [ebx+usb_device_data.Status.Status], al
  784.         mov     [ebx+usb_device_data.DeviceDisconnected], al
  785. ; 2e. There is one reference: a connected USB device.
  786.         inc     eax
  787.         mov     [ebx+usb_device_data.NumReferences], eax
  788. ; 2f. Save handle of configuration pipe for reset recovery.
  789.         mov     eax, [.pipe0]
  790.         mov     [ebx+usb_device_data.ConfigPipe], eax
  791. ; 2g. Save the interface number for configuration requests.
  792.         mov     al, [esi+interface_descr.bInterfaceNumber]
  793.         mov     [ebx+usb_device_data.ConfigRequest+4], al
  794. ; 2h. Initialize common fields in command wrapper.
  795.         mov     [ebx+usb_device_data.Command.Signature], 'USBC'
  796.         mov     [ebx+usb_device_data.Command.Tag], 'xxxx'
  797. ; 2i. Initialize requests queue.
  798.         lea     eax, [ebx+usb_device_data.RequestsQueue]
  799.         mov     [eax+request_queue_item.Next], eax
  800.         mov     [eax+request_queue_item.Prev], eax
  801.         lea     ecx, [ebx+usb_device_data.QueueLock]
  802.         invoke  MutexInit
  803. ; Bulk-only mass storage devices use one OUT bulk endpoint for sending
  804. ; command/data and one IN bulk endpoint for receiving data/status.
  805. ; Look for those endpoints.
  806. ; 3. Get the upper bound of all descriptors' data.
  807.         mov     edx, [.config]  ; configuration descriptor
  808.         movzx   ecx, [edx+config_descr.wTotalLength]
  809.         add     edx, ecx
  810. ; 4. Loop over all descriptors until
  811. ; either end-of-data reached - this is fail
  812. ; or interface descriptor found - this is fail, all further data
  813. ;    correspond to that interface
  814. ; or both endpoint descriptors found.
  815. ; 4a. Loop start: esi points to the interface descriptor,
  816. .lookep:
  817. ; 4b. Get next descriptor.
  818.         movzx   ecx, byte [esi] ; the first byte of all descriptors is length
  819.         add     esi, ecx
  820. ; 4c. Check that at least two bytes are readable. The opposite is an error.
  821.         inc     esi
  822.         cmp     esi, edx
  823.         jae     .errorep
  824.         dec     esi
  825. ; 4d. Check that this descriptor is not interface descriptor. The opposite is
  826. ; an error.
  827.         cmp     byte [esi+endpoint_descr.bDescriptorType], INTERFACE_DESCR_TYPE
  828.         jz      .errorep
  829. ; 4e. Test whether this descriptor is an endpoint descriptor. If not, continue
  830. ; the loop.
  831.         cmp     byte [esi+endpoint_descr.bDescriptorType], ENDPOINT_DESCR_TYPE
  832.         jnz     .lookep
  833. ; 5. Check that the descriptor contains all required data and all data are
  834. ; readable. The opposite is an error.
  835.         cmp     byte [esi+endpoint_descr.bLength], sizeof.endpoint_descr
  836.         jb      .errorep
  837.         lea     ecx, [esi+sizeof.endpoint_descr]
  838.         cmp     ecx, edx
  839.         ja      .errorep
  840. ; 6. Check that the endpoint is bulk endpoint. The opposite is an error.
  841.         mov     cl, [esi+endpoint_descr.bmAttributes]
  842.         and     cl, 3
  843.         cmp     cl, BULK_PIPE
  844.         jnz     .errorep
  845. ; 7. Get the direction of this endpoint.
  846.         movzx   ecx, [esi+endpoint_descr.bEndpointAddress]
  847.         shr     ecx, 7
  848. ; 8. Test whether a pipe for this direction is already opened. If so, continue
  849. ; the loop.
  850.         cmp     [ebx+usb_device_data.OutPipe+ecx*4], 0
  851.         jnz     .lookep
  852. ; 9. Open pipe for this endpoint.
  853. ; 9a. Save registers.
  854.         push    ecx edx
  855. ; 9b. Load parameters from the descriptor.
  856.         movzx   ecx, [esi+endpoint_descr.bEndpointAddress]
  857.         movzx   edx, [esi+endpoint_descr.wMaxPacketSize]
  858.         movzx   eax, [esi+endpoint_descr.bInterval]     ; not used for USB1, may be important for USB2
  859. ; 9c. Call the kernel.
  860.         invoke  USBOpenPipe, [ebx+usb_device_data.ConfigPipe], ecx, edx, BULK_PIPE, eax
  861. ; 9d. Restore registers.
  862.         pop     edx ecx
  863. ; 9e. Check result. If failed, go to 11b.
  864.         test    eax, eax
  865.         jz      .free
  866. ; 9f. Save result.
  867.         mov     [ebx+usb_device_data.OutPipe+ecx*4], eax
  868. ; 10. Test whether the second pipe is already opened. If not, continue loop.
  869.         xor     ecx, 1
  870.         cmp     [ebx+usb_device_data.OutPipe+ecx*4], 0
  871.         jz      .lookep
  872.         jmp     .created
  873. ; 11. An error occured during processing endpoint descriptor.
  874. .errorep:
  875. ; 11a. Print a message.
  876.         DEBUGF 1,'K : error: invalid endpoint descriptor\n'
  877. .free:
  878. ; 11b. Free the allocated usb_device_data.
  879.         xchg    eax, ebx
  880.         invoke  Kfree
  881. .nothing:
  882. ; 11c. Return an error.
  883.         xor     eax, eax
  884.         jmp     .return
  885. .created:
  886. ; 12. Pipes are opened. Send GetMaxLUN control request.
  887.         lea     eax, [ebx+usb_device_data.ConfigRequest]
  888.         mov     byte [eax], 0A1h        ; class request from interface
  889.         mov     byte [eax+1], REQUEST_GETMAXLUN
  890.         mov     byte [eax+6], 1         ; transfer 1 byte
  891.         lea     ecx, [ebx+usb_device_data.MaxLUN]
  892. if DUMP_PACKETS
  893.         DEBUGF 1,'K : GETMAXLUN: %x %x %x %x %x %x %x %x\n',[eax]:2,[eax+1]:2,[eax+2]:2,[eax+3]:2,[eax+4]:2,[eax+5]:2,[eax+6]:2,[eax+7]:2
  894. end if
  895.         invoke  USBControlTransferAsync, [ebx+usb_device_data.ConfigPipe], eax, ecx, 1, known_lun_callback, ebx, 0
  896. ; 13. Return with pointer to device data as returned value.
  897.         xchg    eax, ebx
  898. .return:
  899.         pop     esi ebx
  900.         ret     12
  901. endp
  902.  
  903. ; This function is called when REQUEST_GETMAXLUN is done,
  904. ; either successful or unsuccessful.
  905. proc known_lun_callback
  906.         push    ebx esi
  907. virtual at esp
  908.                 rd      2       ; saved registers
  909.                 dd      ?       ; return address
  910. .pipe           dd      ?
  911. .status         dd      ?
  912. .buffer         dd      ?
  913. .length         dd      ?
  914. .calldata       dd      ?
  915. end virtual
  916. ; 1. Check the status. If the request failed, assume that MaxLUN is zero.
  917.         mov     ebx, [.calldata]
  918.         mov     eax, [.status]
  919.         test    eax, eax
  920.         jz      @f
  921.         DEBUGF 1, 'K : GETMAXLUN failed with status %d, assuming zero\n', eax
  922.         mov     [ebx+usb_device_data.MaxLUN], 0
  923. @@:
  924. ; 2. Allocate the memory for logical devices.
  925.         mov     eax, [ebx+usb_device_data.MaxLUN]
  926.         inc     eax
  927.         DEBUGF 1,'K : %d logical unit(s)\n',eax
  928.         imul    eax, sizeof.usb_unit_data
  929.         push    ebx
  930.         invoke  Kmalloc
  931.         pop     ebx
  932. ; If failed, print a message and do nothing.
  933.         test    eax, eax
  934.         jnz     @f
  935.         mov     esi, nomemory
  936.         invoke  SysMsgBoardStr
  937.         pop     esi ebx
  938.         ret     20
  939. @@:
  940.         mov     [ebx+usb_device_data.LogicalDevices], eax
  941. ; 3. Initialize logical devices and initiate TEST_UNIT_READY request.
  942.         xchg    esi, eax
  943.         xor     ecx, ecx
  944. .looplun:
  945.         mov     [esi+usb_unit_data.Parent], ebx
  946.         mov     [esi+usb_unit_data.LUN], cl
  947.         xor     eax, eax
  948.         mov     [esi+usb_unit_data.MediaPresent], al
  949.         mov     [esi+usb_unit_data.DiskDevice], eax
  950.         mov     [esi+usb_unit_data.SectorSize], eax
  951.         mov     [esi+usb_unit_data.UnitReadyAttempts], eax
  952.         push    ecx
  953.         invoke  GetTimerTicks
  954.         mov     [esi+usb_unit_data.TimerTicks], eax
  955.         stdcall queue_request, ebx, test_unit_ready_req, 0, test_unit_ready_callback, esi
  956.         pop     ecx
  957.         inc     ecx
  958.         add     esi, sizeof.usb_unit_data
  959.         cmp     ecx, [ebx+usb_device_data.MaxLUN]
  960.         jbe     .looplun
  961. ; 4. Return.
  962.         pop     esi ebx
  963.         ret     20
  964. endp
  965.  
  966. ; Builder for SCSI INQUIRY request.
  967. ; edx = first argument = pointer to usb_device_data.Command,
  968. ; second argument = custom data given to queue_request.
  969. proc inquiry_req
  970.         mov     eax, [esp+8]
  971.         mov     al, [eax+usb_unit_data.LUN]
  972.         mov     [edx+command_block_wrapper.Length], sizeof.inquiry_data
  973.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_IN
  974.         mov     [edx+command_block_wrapper.LUN], al
  975.         mov     byte [edx+command_block_wrapper.Command+0], SCSI_INQUIRY
  976.         mov     byte [edx+command_block_wrapper.Command+4], sizeof.inquiry_data
  977.         ret     8
  978. endp
  979.  
  980. ; Called when SCSI INQUIRY request is completed.
  981. proc inquiry_callback
  982. ; 1. Check the status.
  983.         mov     ecx, [esp+4]
  984.         cmp     [ecx+usb_device_data.Status.Status], CSW_STATUS_OK
  985.         jnz     .fail
  986. ; 2. The command has completed successfully.
  987. ; Print a message showing device type, ignore anything but block devices.
  988.         mov     al, [ecx+usb_device_data.InquiryData.PeripheralDevice]
  989.         and     al, 1Fh
  990.         DEBUGF 1,'K : peripheral device type is %x\n',al
  991.         test    al, al
  992.         jnz     .nothing
  993.         DEBUGF 1,'K : direct-access mass storage device detected\n'
  994. ; 3. We have found a new disk device. Increment number of references.
  995.         lock inc [ecx+usb_device_data.NumReferences]
  996. ; Unfortunately, we are now in the context of the USB thread,
  997. ; so we can't notify the kernel immediately: it would try to do something
  998. ; with a new disk, those actions would be synchronous and would require
  999. ; waiting for results of USB requests, but we need to exit this callback
  1000. ; to allow the USB thread to continue working and handling those requests.
  1001. ; 4. Thus, create a temporary kernel thread which would do it.
  1002.         mov     edx, [esp+8]
  1003.         push    ebx ecx esi edi
  1004.         movi    ebx, 1
  1005.         mov     ecx, new_disk_thread
  1006.         ; edx = parameter
  1007.         invoke  CreateThread
  1008.         pop     edi esi ecx ebx
  1009.         cmp     eax, -1
  1010.         jnz     .nothing
  1011. ; on error, reverse step 3
  1012.         lock dec [ecx+usb_device_data.NumReferences]
  1013. .nothing:
  1014.         ret     8
  1015. .fail:
  1016. ; 4. The command has failed. Print a message and do nothing.
  1017.         push    esi
  1018.         mov     esi, inquiry_fail
  1019.         invoke  SysMsgBoardStr
  1020.         pop     esi
  1021.         ret     8
  1022. endp
  1023.  
  1024. ; Builder for SCSI TEST_UNIT_READY request.
  1025. ; edx = first argument = pointer to usb_device_data.Command,
  1026. ; second argument = custom data given to queue_request.
  1027. proc test_unit_ready_req
  1028.         mov     eax, [esp+8]
  1029.         mov     al, [eax+usb_unit_data.LUN]
  1030.         mov     [edx+command_block_wrapper.Length], 0
  1031.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_IN
  1032.         mov     [edx+command_block_wrapper.LUN], al
  1033.         ret     8
  1034. endp
  1035.  
  1036. ; Called when SCSI TEST_UNIT_READY request is completed.
  1037. proc test_unit_ready_callback
  1038. virtual at esp
  1039.                 dd      ?       ; return address
  1040. .device         dd      ?
  1041. .calldata       dd      ?
  1042. end virtual
  1043. ; 1. Check the status.
  1044.         mov     ecx, [.device]
  1045.         mov     edx, [.calldata]
  1046.         cmp     [ecx+usb_device_data.Status.Status], CSW_STATUS_OK
  1047.         jnz     .fail
  1048. ; 2. The command has completed successfully,
  1049. ; possibly after some repetitions. Print a debug message showing
  1050. ; number and time of those. Remember that media is ready and go to 4.
  1051.         DEBUGF 1,'K : media is ready\n'
  1052.         invoke  GetTimerTicks
  1053.         sub     eax, [edx+usb_unit_data.TimerTicks]
  1054.         DEBUGF 1,'K : %d attempts, %d ticks\n',[edx+usb_unit_data.UnitReadyAttempts],eax
  1055.         inc     [edx+usb_unit_data.MediaPresent]
  1056.         jmp     .inquiry
  1057. .fail:
  1058. ; 3. The command has failed.
  1059. ; Retry the same request up to 3 times with 10ms delay;
  1060. ; if limit of retries is not reached, exit from the function.
  1061. ; Otherwise, go to 4.
  1062.         inc     [edx+usb_unit_data.UnitReadyAttempts]
  1063.         cmp     [edx+usb_unit_data.UnitReadyAttempts], 3
  1064.         jz      @f
  1065.         push    ecx edx esi
  1066.         movi    esi, 10
  1067.         invoke  Sleep
  1068.         pop     esi edx ecx
  1069.         stdcall queue_request, ecx, test_unit_ready_req, 0, test_unit_ready_callback, edx
  1070.         ret     8
  1071. @@:
  1072.         DEBUGF 1,'K : media not ready\n'
  1073. .inquiry:
  1074. ; 4. initiate INQUIRY request.
  1075.         lea     eax, [ecx+usb_device_data.InquiryData]
  1076.         stdcall queue_request, ecx, inquiry_req, eax, inquiry_callback, edx
  1077.         ret     8
  1078. endp
  1079.  
  1080. ; Temporary thread for initial actions with a new disk device.
  1081. proc new_disk_thread
  1082.         sub     esp, 32
  1083. virtual at esp
  1084. .name   rb      32      ; device name
  1085. .param  dd      ?       ; contents of edx at the moment of int 0x40/eax=51
  1086.         dd      ?       ; stack segment
  1087. end virtual
  1088. ; We are ready to notify the kernel about a new disk device.
  1089.         mov     esi, [.param]
  1090. ; 1. Generate name.
  1091. ; 1a. Find a free index.
  1092.         mov     ecx, free_numbers_lock
  1093.         invoke  MutexLock
  1094.         xor     eax, eax
  1095. @@:
  1096.         bsf     edx, [free_numbers+eax]
  1097.         jnz     @f
  1098.         add     eax, 4
  1099.         cmp     eax, 4*4
  1100.         jnz     @b
  1101.         invoke  MutexUnlock
  1102.         push    esi
  1103.         mov     esi, noindex
  1104.         invoke  SysMsgBoardStr
  1105.         pop     esi
  1106.         jmp     .drop_reference
  1107. @@:
  1108. ; 1b. Mark the index as busy.
  1109.         btr     [free_numbers+eax], edx
  1110.         lea     eax, [eax*8+edx]
  1111.         push    eax
  1112.         invoke  MutexUnlock
  1113.         pop     eax
  1114. ; 1c. Generate a name of the form "usbhd<index>" in the stack.
  1115.         mov     dword [esp], 'usbh'
  1116.         lea     edi, [esp+5]
  1117.         mov     byte [edi-1], 'd'
  1118.         push    eax
  1119.         push    -'0'
  1120.         movi    ecx, 10
  1121. @@:
  1122.         cdq
  1123.         div     ecx
  1124.         push    edx
  1125.         test    eax, eax
  1126.         jnz     @b
  1127. @@:
  1128.         pop     eax
  1129.         add     al, '0'
  1130.         stosb
  1131.         jnz     @b
  1132.         pop     ecx
  1133.         mov     edx, esp
  1134. ; 3d. Store the index in usb_unit_data to free it later.
  1135.         mov     [esi+usb_unit_data.DiskIndex], cl
  1136. ; 4. Notify the kernel about a new disk.
  1137. ; 4a. Add a disk.
  1138. ;       stdcall queue_request, ecx, read_capacity_req, eax, read_capacity_callback, eax
  1139.         invoke  DiskAdd, disk_functions, edx, esi, 0
  1140.         mov     ebx, eax
  1141. ; 4b. If it failed, release the index and do nothing.
  1142.         test    eax, eax
  1143.         jz      .free_index
  1144. ; 4c. Notify the kernel that a media is present.
  1145.         invoke  DiskMediaChanged, eax, 1
  1146. ; 5. Lock the requests queue, check that device is not disconnected,
  1147. ; store the disk handle, unlock the requests queue.
  1148.         mov     ecx, [esi+usb_unit_data.Parent]
  1149.         add     ecx, usb_device_data.QueueLock
  1150.         invoke  MutexLock
  1151.         cmp     byte [ecx+usb_device_data.DeviceDisconnected-usb_device_data.QueueLock], 0
  1152.         jnz     .disconnected
  1153.         mov     [esi+usb_unit_data.DiskDevice], ebx
  1154.         invoke  MutexUnlock
  1155.         jmp     .exit
  1156. .disconnected:
  1157.         invoke  MutexUnlock
  1158.         stdcall disk_close, ebx
  1159.         jmp     .exit
  1160. .free_index:
  1161.         mov     ecx, free_numbers_lock
  1162.         invoke  MutexLock
  1163.         movzx   eax, [esi+usb_unit_data.DiskIndex]
  1164.         bts     [free_numbers], eax
  1165.         invoke  MutexUnlock
  1166. .drop_reference:
  1167.         mov     esi, [esi+usb_unit_data.Parent]
  1168.         lock dec [esi+usb_device_data.NumReferences]
  1169.         jnz     .exit
  1170.         mov     eax, [esi+usb_device_data.LogicalDevices]
  1171.         invoke  Kfree
  1172.         xchg    eax, esi
  1173.         invoke  Kfree
  1174. .exit:
  1175.         or      eax, -1
  1176.         int     0x40
  1177. endp
  1178.  
  1179. ; This function is called when the device is disconnected.
  1180. proc DeviceDisconnected
  1181.         push    ebx esi
  1182. virtual at esp
  1183.         rd      2       ; saved registers
  1184.         dd      ?       ; return address
  1185. .device dd      ?
  1186. end virtual
  1187. ; 1. Say a message.
  1188.         mov     esi, disconnectmsg
  1189.         invoke  SysMsgBoardStr
  1190. ; 2. Lock the requests queue, set .DeviceDisconnected to 1,
  1191. ; unlock the requests queue.
  1192. ; Locking is required for synchronization with queue_request:
  1193. ; all USB callbacks are executed in the same thread and are
  1194. ; synchronized automatically, but queue_request can be running
  1195. ; from any thread which wants to do something with a filesystem.
  1196. ; Without locking, it would be possible that queue_request has
  1197. ; been started, has checked that device is not yet disconnected,
  1198. ; then DeviceDisconnected completes and all handles become invalid,
  1199. ; then queue_request tries to use them.
  1200.         mov     esi, [.device]
  1201.         lea     ecx, [esi+usb_device_data.QueueLock]
  1202.         invoke  MutexLock
  1203.         mov     [esi+usb_device_data.DeviceDisconnected], 1
  1204.         invoke  MutexUnlock
  1205. ; 3. Drop one reference to the structure and check whether
  1206. ; that was the last reference.
  1207.         lock dec [esi+usb_device_data.NumReferences]
  1208.         jz      .free
  1209. ; 4. If not, there are some additional references due to disk devices;
  1210. ; notify the kernel that those disks are deleted.
  1211. ; Note that new disks cannot be added while we are looping here,
  1212. ; because new_disk_thread checks for .DeviceDisconnected.
  1213.         mov     ebx, [esi+usb_device_data.MaxLUN]
  1214.         mov     esi, [esi+usb_device_data.LogicalDevices]
  1215.         inc     ebx
  1216. .diskdel:
  1217.         mov     eax, [esi+usb_unit_data.DiskDevice]
  1218.         test    eax, eax
  1219.         jz      @f
  1220.         invoke  DiskDel, eax
  1221. @@:
  1222.         add     esi, sizeof.usb_unit_data
  1223.         dec     ebx
  1224.         jnz     .diskdel
  1225. ; In this case, some operations with those disks are still possible,
  1226. ; so we can't do anything more now. disk_close will take care of the rest.
  1227. .return:
  1228.         pop     esi ebx
  1229.         ret     4
  1230. ; 5. If there are no disk devices, free all resources which were allocated.
  1231. .free:
  1232.         mov     eax, [esi+usb_device_data.LogicalDevices]
  1233.         test    eax, eax
  1234.         jz      @f
  1235.         invoke  Kfree
  1236. @@:
  1237.         xchg    eax, esi
  1238.         invoke  Kfree
  1239.         jmp     .return
  1240. endp
  1241.  
  1242. ; Disk functions.
  1243. DISK_STATUS_OK              = 0 ; success
  1244. DISK_STATUS_GENERAL_ERROR   = -1; if no other code is suitable
  1245. DISK_STATUS_INVALID_CALL    = 1 ; invalid input parameters
  1246. DISK_STATUS_NO_MEDIA        = 2 ; no media present
  1247. DISK_STATUS_END_OF_MEDIA    = 3 ; end of media while reading/writing data
  1248.  
  1249. ; Called when all operations with the given disk are done.
  1250. proc disk_close
  1251.         push    ebx esi
  1252. virtual at esp
  1253.         rd      2       ; saved registers
  1254.         dd      ?       ; return address
  1255. .userdata       dd      ?
  1256. end virtual
  1257.         mov     esi, [.userdata]
  1258.         mov     ecx, free_numbers_lock
  1259.         invoke  MutexLock
  1260.         movzx   eax, [esi+usb_unit_data.DiskIndex]
  1261.         bts     [free_numbers], eax
  1262.         invoke  MutexUnlock
  1263.         mov     esi, [esi+usb_unit_data.Parent]
  1264.         lock dec [esi+usb_device_data.NumReferences]
  1265.         jnz     .nothing
  1266.         mov     eax, [esi+usb_device_data.LogicalDevices]
  1267.         invoke  Kfree
  1268.         xchg    eax, esi
  1269.         invoke  Kfree
  1270. .nothing:
  1271.         pop     esi ebx
  1272.         ret     4
  1273. endp
  1274.  
  1275. ; Returns sector size, capacity and flags of the media.
  1276. proc disk_querymedia stdcall uses ebx esi edi, \
  1277.         userdata:dword, mediainfo:dword
  1278. ; 1. Create event for waiting.
  1279.         xor     esi, esi
  1280.         xor     ecx, ecx
  1281.         invoke  CreateEvent
  1282.         test    eax, eax
  1283.         jz      .generic_fail
  1284.         push    eax
  1285.         push    edx
  1286.         push    ecx
  1287.         push    0
  1288.         push    0
  1289. virtual at ebp-.localsize
  1290. .locals:
  1291. ; two following dwords are the output of READ_CAPACITY
  1292. .LastLBABE      dd      ?
  1293. .SectorSizeBE   dd      ?
  1294. .Status         dd      ?
  1295. ; two following dwords identify an event
  1296. .event_code     dd      ?
  1297. .event          dd      ?
  1298.                 rd      3       ; saved registers
  1299. .localsize = $ - .locals
  1300.                 dd      ?       ; saved ebp
  1301.                 dd      ?       ; return address
  1302. .userdata       dd      ?
  1303. .mediainfo      dd      ?
  1304. end virtual
  1305. ; 2. Initiate SCSI READ_CAPACITY request.
  1306.         mov     eax, [userdata]
  1307.         mov     ecx, [eax+usb_unit_data.Parent]
  1308.         mov     edx, esp
  1309.         stdcall queue_request, ecx, read_capacity_req, edx, read_capacity_callback, edx
  1310. ; 3. Wait for event. This destroys it.
  1311.         mov     eax, [.event]
  1312.         mov     ebx, [.event_code]
  1313.         invoke  WaitEvent
  1314. ; 4. Get the status and results.
  1315.         pop     ecx
  1316.         bswap   ecx     ; .LastLBA
  1317.         pop     edx
  1318.         bswap   edx     ; .SectorSize
  1319.         pop     eax     ; .Status
  1320. ; 5. If the request has completed successfully, store results.
  1321.         test    eax, eax
  1322.         jnz     @f
  1323.         DEBUGF 1,'K : sector size is %d, last sector is %d\n',edx,ecx
  1324.         mov     ebx, [mediainfo]
  1325.         mov     [ebx], eax      ; flags = 0
  1326.         mov     [ebx+4], edx    ; sectorsize
  1327.         add     ecx, 1
  1328.         adc     eax, 0
  1329.         mov     [ebx+8], ecx
  1330.         mov     [ebx+12], eax   ; capacity
  1331.         mov     eax, [userdata]
  1332.         mov     [eax+usb_unit_data.SectorSize], edx
  1333.         xor     eax, eax
  1334. @@:
  1335. ; 6. Restore the stack and return.
  1336.         pop     ecx
  1337.         pop     ecx
  1338.         ret
  1339. .generic_fail:
  1340.         or      eax, -1
  1341.         ret
  1342. endp
  1343.  
  1344. ; Builder for SCSI READ_CAPACITY request.
  1345. ; edx = first argument = pointer to usb_device_data.Command,
  1346. ; second argument = custom data given to queue_request,
  1347. ; pointer to disk_querymedia.locals.
  1348. proc read_capacity_req
  1349.         mov     eax, [esp+8]
  1350.         mov     eax, [eax+disk_querymedia.userdata-disk_querymedia.locals]
  1351.         mov     al, [eax+usb_unit_data.LUN]
  1352.         mov     [edx+command_block_wrapper.Length], 8
  1353.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_IN
  1354.         mov     [edx+command_block_wrapper.LUN], al
  1355.         mov     byte [edx+command_block_wrapper.Command+0], SCSI_READ_CAPACITY
  1356.         ret     8
  1357. endp
  1358.  
  1359. ; Called when SCSI READ_CAPACITY request is completed.
  1360. proc read_capacity_callback
  1361. ; Transform the status to return value of disk_querymedia
  1362. ; and set the event.
  1363.         mov     ecx, [esp+4]
  1364.         xor     eax, eax
  1365.         cmp     [ecx+usb_device_data.Status.Status], al
  1366.         jz      @f
  1367.         or      eax, -1
  1368. @@:
  1369.         mov     ecx, [esp+8]
  1370.         mov     [ecx+disk_querymedia.Status-disk_querymedia.locals], eax
  1371.         push    ebx esi edi
  1372.         mov     eax, [ecx+disk_querymedia.event-disk_querymedia.locals]
  1373.         mov     ebx, [ecx+disk_querymedia.event_code-disk_querymedia.locals]
  1374.         xor     edx, edx
  1375.         xor     esi, esi
  1376.         invoke  RaiseEvent
  1377.         pop     edi esi ebx
  1378.         ret     8
  1379. endp
  1380.  
  1381. disk_write:
  1382.         mov     al, SCSI_WRITE10
  1383.         jmp     disk_read_write
  1384.  
  1385. disk_read:
  1386.         mov     al, SCSI_READ10
  1387.  
  1388. ; Reads from the device or writes to the device.
  1389. proc disk_read_write stdcall uses ebx esi edi, \
  1390.         userdata:dword, buffer:dword, startsector:qword, numsectors:dword
  1391. ; 1. Initialize.
  1392.         push    eax     ; .command
  1393.         mov     eax, [userdata]
  1394.         mov     eax, [eax+usb_unit_data.SectorSize]
  1395.         push    eax     ; .SectorSize
  1396.         push    0       ; .processed
  1397.         mov     eax, [numsectors]
  1398.         mov     eax, [eax]
  1399. ; 2. The transfer length for SCSI_{READ,WRITE}10 commands can not be greater
  1400. ; than 0xFFFF, so split the request to slices with <= 0xFFFF sectors.
  1401. max_sectors_at_time = 0xFFFF
  1402. .split:
  1403.         push    eax     ; .length_rest
  1404.         cmp     eax, max_sectors_at_time
  1405.         jb      @f
  1406.         mov     eax, max_sectors_at_time
  1407. @@:
  1408.         sub     [esp], eax
  1409.         push    eax     ; .length_cur
  1410. ; 3. startsector must fit in 32 bits, otherwise abort the request.
  1411.         cmp     dword [startsector+4], 0
  1412.         jnz     .generic_fail
  1413. ; 4. Create event for waiting.
  1414.         xor     esi, esi
  1415.         xor     ecx, ecx
  1416.         invoke  CreateEvent
  1417.         test    eax, eax
  1418.         jz      .generic_fail
  1419.         push    eax     ; .event
  1420.         push    edx     ; .event_code
  1421.         push    ecx     ; .status
  1422. virtual at ebp-.localsize
  1423. .locals:
  1424. .status         dd      ?
  1425. .event_code     dd      ?
  1426. .event          dd      ?
  1427. .length_cur     dd      ?
  1428. .length_rest    dd      ?
  1429. .processed      dd      ?
  1430. .SectorSize     dd      ?
  1431. .command        db      ?
  1432.                 rb      3
  1433.                 rd      3       ; saved registers
  1434. .localsize = $ - .locals
  1435.                 dd      ?       ; saved ebp
  1436.                 dd      ?       ; return address
  1437. .userdata       dd      ?
  1438. .buffer         dd      ?
  1439. .startsector    dq      ?
  1440. .numsectors     dd      ?
  1441. end virtual
  1442. ; 5. Initiate SCSI READ10 or WRITE10 request.
  1443.         mov     eax, [userdata]
  1444.         mov     ecx, [eax+usb_unit_data.Parent]
  1445.         stdcall queue_request, ecx, read_write_req, [buffer], read_write_callback, esp
  1446. ; 6. Wait for event. This destroys it.
  1447.         mov     eax, [.event]
  1448.         mov     ebx, [.event_code]
  1449.         invoke  WaitEvent
  1450. ; 7. Get the status. If the operation has failed, abort.
  1451.         pop     eax     ; .status
  1452.         pop     ecx ecx ; cleanup .event_code, .event
  1453.         pop     ecx     ; .length_cur
  1454.         test    eax, eax
  1455.         jnz     .return
  1456. ; 8. Otherwise, continue the loop started at step 2.
  1457.         add     dword [startsector], ecx
  1458.         adc     dword [startsector+4], eax
  1459.         imul    ecx, [.SectorSize]
  1460.         add     [buffer], ecx
  1461.         pop     eax
  1462.         test    eax, eax
  1463.         jnz     .split
  1464.         push    eax
  1465. .return:
  1466. ; 9. Restore the stack, store .processed to [numsectors], return.
  1467.         pop     ecx     ; .length_rest
  1468.         pop     ecx     ; .processed
  1469.         mov     edx, [numsectors]
  1470.         mov     [edx], ecx
  1471.         pop     ecx     ; .SectorSize
  1472.         pop     ecx     ; .command
  1473.         ret
  1474. .generic_fail:
  1475.         or      eax, -1
  1476.         pop     ecx     ; .length_cur
  1477.         jmp     .return
  1478. endp
  1479.  
  1480. ; Builder for SCSI READ10 or WRITE10 request.
  1481. ; edx = first argument = pointer to usb_device_data.Command,
  1482. ; second argument = custom data given to queue_request,
  1483. ; pointer to disk_read_write.locals.
  1484. proc read_write_req
  1485.         mov     eax, [esp+8]
  1486.         mov     ecx, [eax+disk_read_write.userdata-disk_read_write.locals]
  1487.         mov     cl, [ecx+usb_unit_data.LUN]
  1488.         mov     [edx+command_block_wrapper.LUN], cl
  1489.         mov     ecx, [eax+disk_read_write.length_cur-disk_read_write.locals]
  1490.         imul    ecx, [eax+disk_read_write.SectorSize-disk_read_write.locals]
  1491.         mov     [edx+command_block_wrapper.Length], ecx
  1492.         mov     cl, [eax+disk_read_write.command-disk_read_write.locals]
  1493.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_OUT
  1494.         cmp     cl, SCSI_READ10
  1495.         jnz     @f
  1496.         mov     [edx+command_block_wrapper.Flags], CBW_FLAG_IN
  1497. @@:
  1498.         mov     byte [edx+command_block_wrapper.Command], cl
  1499.         mov     ecx, dword [eax+disk_read_write.startsector-disk_read_write.locals]
  1500.         bswap   ecx
  1501.         mov     dword [edx+command_block_wrapper.Command+2], ecx
  1502.         mov     ecx, [eax+disk_read_write.length_cur-disk_read_write.locals]
  1503.         xchg    cl, ch
  1504.         mov     word [edx+command_block_wrapper.Command+7], cx
  1505.         ret     8
  1506. endp
  1507.  
  1508. ; Called when SCSI READ10 or WRITE10 request is completed.
  1509. proc read_write_callback
  1510. ; 1. Initialize.
  1511.         push    ebx esi edi
  1512. virtual at esp
  1513.         rd      3       ; saved registers
  1514.         dd      ?       ; return address
  1515. .device         dd      ?
  1516. .calldata       dd      ?
  1517. end virtual
  1518.         mov     ecx, [.device]
  1519.         mov     esi, [.calldata]
  1520. ; 2. Get the number of sectors which were read.
  1521. ; If the status is OK or FAIL, the field .LengthRest is valid.
  1522. ; Otherwise, it is invalid, so assume zero sectors.
  1523.         xor     eax, eax
  1524.         cmp     [ecx+usb_device_data.Status.Status], CSW_STATUS_FAIL
  1525.         ja      .sectors_calculated
  1526.         mov     eax, [ecx+usb_device_data.LengthRest]
  1527.         xor     edx, edx
  1528.         div     [esi+disk_read_write.SectorSize-disk_read_write.locals]
  1529.         test    edx, edx
  1530.         jz      @f
  1531.         inc     eax
  1532. @@:
  1533.         mov     edx, eax
  1534.         mov     eax, [esi+disk_read_write.length_cur-disk_read_write.locals]
  1535.         sub     eax, edx
  1536.         jae     .sectors_calculated
  1537.         xor     eax, eax
  1538. .sectors_calculated:
  1539. ; 3. Increase the total number of processed sectors.
  1540.         add     [esi+disk_read_write.processed-disk_read_write.locals], eax
  1541. ; 4. Set status to OK if all sectors were read, to ERROR otherwise.
  1542.         cmp     eax, [esi+disk_read_write.length_cur-disk_read_write.locals]
  1543.         setz    al
  1544.         movzx   eax, al
  1545.         dec     eax
  1546.         mov     [esi+disk_read_write.status-disk_read_write.locals], eax
  1547. ; 5. Set the event.
  1548.         mov     eax, [esi+disk_read_write.event-disk_read_write.locals]
  1549.         mov     ebx, [esi+disk_read_write.event_code-disk_read_write.locals]
  1550.         xor     edx, edx
  1551.         xor     esi, esi
  1552.         invoke  RaiseEvent
  1553. ; 6. Return.
  1554.         pop     edi esi ebx
  1555.         ret     8
  1556. endp
  1557.  
  1558. ; strings
  1559. my_driver       db      'usbstor',0
  1560. disconnectmsg   db      'K : USB mass storage device disconnected',13,10,0
  1561. nomemory        db      'K : no memory',13,10,0
  1562. unkdevice       db      'K : unknown mass storage device',13,10,0
  1563. okdevice        db      'K : USB mass storage device detected',13,10,0
  1564. transfererror   db      'K : USB transfer error, disabling mass storage',13,10,0
  1565. invresponse     db      'K : invalid response from mass storage device',13,10,0
  1566. fatalerr        db      'K : mass storage device reports fatal error',13,10,0
  1567. inquiry_fail    db      'K : INQUIRY command failed',13,10,0
  1568. ;read_capacity_fail db  'K : READ CAPACITY command failed',13,10,0
  1569. ;read_fail      db      'K : READ command failed',13,10,0
  1570. noindex         db      'K : failed to generate disk name',13,10,0
  1571.  
  1572. align 4
  1573. ; Structure with callback functions.
  1574. usb_functions:
  1575.         dd      usb_functions_end - usb_functions
  1576.         dd      AddDevice
  1577.         dd      DeviceDisconnected
  1578. usb_functions_end:
  1579.  
  1580. disk_functions:
  1581.         dd      disk_functions_end - disk_functions
  1582.         dd      disk_close
  1583.         dd      0       ; closemedia
  1584.         dd      disk_querymedia
  1585.         dd      disk_read
  1586.         dd      disk_write
  1587.         dd      0       ; flush
  1588.         dd      0       ; adjust_cache_size: use default cache
  1589. disk_functions_end:
  1590.  
  1591. data fixups
  1592. end data
  1593.  
  1594. free_numbers_lock       rd      3
  1595. ; 128 devices should be enough for everybody
  1596. free_numbers    dd      -1, -1, -1, -1
  1597.  
  1598. ; for DEBUGF macro
  1599. include_debug_strings
  1600.