]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c
MdeModulePkg: Replace BSD License with BSD+Patent License
[mirror_edk2.git] / MdeModulePkg / Bus / Usb / UsbMassStorageDxe / UsbMassBoot.c
index 980f8b289510854a88f826021d37efd7e98e87cb..600896b6a2147c6c5cc17356a842b44347000434 100644 (file)
 /** @file\r
+  Implementation of the command set of USB Mass Storage Specification\r
+  for Bootability, Revision 1.0.\r
 \r
-Copyright (c) 2007, Intel Corporation\r
-All rights reserved. This program and the accompanying materials\r
-are licensed and made available under the terms and conditions of the BSD License\r
-which accompanies this distribution.  The full text of the license may be found at\r
-http://opensource.org/licenses/bsd-license.php\r
-\r
-THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
-\r
-Module Name:\r
-\r
-  UsbMassBoot.c\r
-\r
-Abstract:\r
-\r
-  This file implement the command set of "USB Mass Storage Specification\r
-  for Bootability".\r
-\r
-Revision History\r
-\r
-\r
-**/\r
-\r
-#include "UsbMassImpl.h"\r
-\r
-\r
-/**\r
-  Read an UINT32 from the buffer to avoid byte alignment problems, then\r
-  convert that to the little endia. The USB mass storage bootability spec\r
-  use big endia\r
-\r
-  @param  Buf                    The buffer contains the first byte of the UINT32\r
-                                 in big endia.\r
-\r
-  @return The UINT32 value read from the buffer in little endia.\r
-\r
-**/\r
-STATIC\r
-UINT32\r
-UsbBootGetUint32 (\r
-  IN UINT8                  *Buf\r
-  )\r
-{\r
-  UINT32                    Value;\r
-\r
-  CopyMem (&Value, Buf, sizeof (UINT32));\r
-  return USB_BOOT_SWAP32 (Value);\r
-}\r
-\r
-\r
-/**\r
-  Put an UINT32 in little endia to the buffer. The data is converted to\r
-  big endia before writing.\r
-\r
-  @param  Buf                    The buffer to write data to\r
-  @param  Data32                 The data to write.\r
-\r
-  @return None\r
-\r
-**/\r
-STATIC\r
-VOID\r
-UsbBootPutUint32 (\r
-  IN UINT8                  *Buf,\r
-  IN UINT32                 Data32\r
-  )\r
-{\r
-  Data32 = USB_BOOT_SWAP32 (Data32);\r
-  CopyMem (Buf, &Data32, sizeof (UINT32));\r
-}\r
-\r
-\r
-/**\r
-  Put an UINT16 in little endia to the buffer. The data is converted to\r
-  big endia before writing.\r
-\r
-  @param  Buf                    The buffer to write data to\r
-  @param  Data16                 The data to write\r
-\r
-  @return None\r
+Copyright (c) 2007 - 2018, Intel Corporation. All rights reserved.<BR>\r
+SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
-STATIC\r
-VOID\r
-UsbBootPutUint16 (\r
-  IN UINT8                   *Buf,\r
-  IN UINT16                  Data16\r
-  )\r
-{\r
-  Data16 = USB_BOOT_SWAP16 (Data16);\r
-  CopyMem (Buf, &Data16, sizeof (UINT16));\r
-}\r
 \r
+#include "UsbMass.h"\r
 \r
 /**\r
-  Request sense information via sending Request Sense\r
-  Packet Command.\r
+  Execute REQUEST SENSE Command to retrieve sense data from device.\r
 \r
-  @param  UsbMass                The device to be requested sense data\r
+  @param  UsbMass                The device whose sense data is requested.\r
 \r
-  @retval EFI_DEVICE_ERROR       Hardware error\r
-  @retval EFI_SUCCESS            Success\r
+  @retval EFI_SUCCESS            The command is executed successfully.\r
+  @retval EFI_DEVICE_ERROR       Failed to request sense.\r
+  @retval EFI_NO_RESPONSE        The device media doesn't response this request.\r
+  @retval EFI_INVALID_PARAMETER  The command has some invalid parameters.\r
+  @retval EFI_WRITE_PROTECTED    The device is write protected.\r
+  @retval EFI_MEDIA_CHANGED      The device media has been changed.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -119,14 +37,14 @@ UsbBootRequestSense (
   Transport = UsbMass->Transport;\r
 \r
   //\r
-  // Request the sense data from the device if command failed\r
+  // Request the sense data from the device\r
   //\r
   ZeroMem (&SenseCmd, sizeof (USB_BOOT_REQUEST_SENSE_CMD));\r
   ZeroMem (&SenseData, sizeof (USB_BOOT_REQUEST_SENSE_DATA));\r
 \r
   SenseCmd.OpCode   = USB_BOOT_REQUEST_SENSE_OPCODE;\r
-  SenseCmd.Lun      = USB_BOOT_LUN (UsbMass->Lun);\r
-  SenseCmd.AllocLen = sizeof (USB_BOOT_REQUEST_SENSE_DATA);\r
+  SenseCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
+  SenseCmd.AllocLen = (UINT8) sizeof (USB_BOOT_REQUEST_SENSE_DATA);\r
 \r
   Status = Transport->ExecCommand (\r
                         UsbMass->Context,\r
@@ -135,22 +53,37 @@ UsbBootRequestSense (
                         EfiUsbDataIn,\r
                         &SenseData,\r
                         sizeof (USB_BOOT_REQUEST_SENSE_DATA),\r
+                        UsbMass->Lun,\r
                         USB_BOOT_GENERAL_CMD_TIMEOUT,\r
                         &CmdResult\r
                         );\r
   if (EFI_ERROR (Status) || CmdResult != USB_MASS_CMD_SUCCESS) {\r
-    DEBUG ((mUsbMscError, "UsbBootRequestSense: (%r) CmdResult=0x%x\n", Status, CmdResult));\r
-    return EFI_DEVICE_ERROR;\r
+    DEBUG ((EFI_D_ERROR, "UsbBootRequestSense: (%r) CmdResult=0x%x\n", Status, CmdResult));\r
+    if (!EFI_ERROR (Status)) {\r
+      Status = EFI_DEVICE_ERROR;\r
+    }\r
+    return Status;\r
   }\r
 \r
   //\r
-  // Interpret the sense data and update the media status if necessary.\r
+  // If sense data is retrieved successfully, interpret the sense data\r
+  // and update the media status if necessary.\r
   //\r
   Media = &UsbMass->BlockIoMedia;\r
 \r
   switch (USB_BOOT_SENSE_KEY (SenseData.SenseKey)) {\r
 \r
   case USB_BOOT_SENSE_NO_SENSE:\r
+    if (SenseData.Asc == USB_BOOT_ASC_NO_ADDITIONAL_SENSE_INFORMATION) {\r
+      //\r
+      // It is not an error if a device does not have additional sense information\r
+      //\r
+      Status = EFI_SUCCESS;\r
+    } else {\r
+      Status = EFI_NO_RESPONSE;\r
+    }\r
+    break;\r
+\r
   case USB_BOOT_SENSE_RECOVERED:\r
     //\r
     // Suppose hardware can handle this case, and recover later by itself\r
@@ -159,26 +92,12 @@ UsbBootRequestSense (
     break;\r
 \r
   case USB_BOOT_SENSE_NOT_READY:\r
-    switch (SenseData.ASC) {\r
-    case USB_BOOT_ASC_NO_MEDIA:\r
-      Status              = EFI_NO_MEDIA;\r
-      Media->MediaPresent = FALSE;\r
-      break;\r
-\r
-    case USB_BOOT_ASC_MEDIA_UPSIDE_DOWN:\r
-      Status              = EFI_DEVICE_ERROR;\r
+    Status = EFI_DEVICE_ERROR;\r
+    if (SenseData.Asc == USB_BOOT_ASC_NO_MEDIA) {\r
       Media->MediaPresent = FALSE;\r
-      break;\r
-\r
-    case USB_BOOT_ASC_NOT_READY:\r
-      if (SenseData.ASCQ == USB_BOOT_ASCQ_IN_PROGRESS ||\r
-          SenseData.ASCQ == USB_BOOT_ASCQ_DEVICE_BUSY) {\r
-        //\r
-        // Regular timeout, and need retry once more\r
-        //\r
-        DEBUG ((mUsbMscInfo, "UsbBootRequestSense: Not ready and need retry once more\n"));\r
-        Status = EFI_NOT_READY;\r
-      }\r
+      Status = EFI_NO_MEDIA;\r
+    } else if (SenseData.Asc == USB_BOOT_ASC_NOT_READY) {\r
+      Status = EFI_NOT_READY;\r
     }\r
     break;\r
 \r
@@ -188,15 +107,23 @@ UsbBootRequestSense (
 \r
   case USB_BOOT_SENSE_UNIT_ATTENTION:\r
     Status = EFI_DEVICE_ERROR;\r
-    if (SenseData.ASC == USB_BOOT_ASC_MEDIA_CHANGE) {\r
+    if (SenseData.Asc == USB_BOOT_ASC_MEDIA_CHANGE) {\r
+      //\r
+      // If MediaChange, reset ReadOnly and new MediaId\r
+      //\r
       Status = EFI_MEDIA_CHANGED;\r
-      UsbMass->BlockIoMedia.MediaId++;\r
+      Media->ReadOnly = FALSE;\r
+      Media->MediaId++;\r
+    } else if (SenseData.Asc == USB_BOOT_ASC_NOT_READY) {\r
+      Status = EFI_NOT_READY;\r
+    } else if (SenseData.Asc == USB_BOOT_ASC_NO_MEDIA) {\r
+      Status = EFI_NOT_READY;\r
     }\r
     break;\r
 \r
-  case USB_BOOT_SNESE_DATA_PROTECT:\r
-    Status                          = EFI_WRITE_PROTECTED;\r
-    UsbMass->BlockIoMedia.ReadOnly  = TRUE;\r
+  case USB_BOOT_SENSE_DATA_PROTECT:\r
+    Status = EFI_WRITE_PROTECTED;\r
+    Media->ReadOnly = TRUE;\r
     break;\r
 \r
   default:\r
@@ -204,11 +131,12 @@ UsbBootRequestSense (
     break;\r
   }\r
 \r
-  DEBUG ((mUsbMscInfo, "UsbBootRequestSense: (%r) with sense key %x/%x/%x\n",\r
+  DEBUG ((EFI_D_INFO, "UsbBootRequestSense: (%r) with error code (%x) sense key %x/%x/%x\n",\r
           Status,\r
+          SenseData.ErrorCode,\r
           USB_BOOT_SENSE_KEY (SenseData.SenseKey),\r
-          SenseData.ASC,\r
-          SenseData.ASCQ\r
+          SenseData.Asc,\r
+          SenseData.Ascq\r
           ));\r
 \r
   return Status;\r
@@ -216,9 +144,11 @@ UsbBootRequestSense (
 \r
 \r
 /**\r
-  Execute the USB mass storage bootability commands. If execution\r
-  failed, retrieve the error by REQUEST_SENSE then update the device's\r
-  status, such as ReadyOnly.\r
+  Execute the USB mass storage bootability commands.\r
+\r
+  This function executes the USB mass storage bootability commands.\r
+  If execution failed, retrieve the error by REQUEST_SENSE, then\r
+  update the device's status, such as ReadyOnly.\r
 \r
   @param  UsbMass                The device to issue commands to\r
   @param  Cmd                    The command to execute\r
@@ -228,14 +158,10 @@ UsbBootRequestSense (
   @param  DataLen                The length of expected data\r
   @param  Timeout                The timeout used to transfer\r
 \r
-  @retval EFI_SUCCESS            The command is excuted OK\r
-  @retval EFI_DEVICE_ERROR       Failed to request sense\r
-  @retval EFI_INVALID_PARAMETER  The command has some invalid parameters\r
-  @retval EFI_WRITE_PROTECTED    The device is write protected\r
-  @retval EFI_MEDIA_CHANGED      The device media has been changed\r
+  @retval EFI_SUCCESS            Command is executed successfully\r
+  @retval Others                 Command execution failed.\r
 \r
 **/\r
-STATIC\r
 EFI_STATUS\r
 UsbBootExecCmd (\r
   IN USB_MASS_DEVICE            *UsbMass,\r
@@ -259,25 +185,39 @@ UsbBootExecCmd (
                            DataDir,\r
                            Data,\r
                            DataLen,\r
+                           UsbMass->Lun,\r
                            Timeout,\r
                            &CmdResult\r
                            );\r
+\r
+  if (Status == EFI_TIMEOUT) {\r
+    DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: %r to Exec 0x%x Cmd\n", Status, *(UINT8 *)Cmd));\r
+    return EFI_TIMEOUT;\r
+  }\r
+\r
   //\r
-  // ExecCommand return success and get the right CmdResult means\r
-  // the commnad transfer is OK.\r
+  // If ExecCommand() returns no error and CmdResult is success,\r
+  // then the commnad transfer is successful.\r
   //\r
-  if ((CmdResult == USB_MASS_CMD_SUCCESS) && !EFI_ERROR(Status)) {\r
+  if ((CmdResult == USB_MASS_CMD_SUCCESS) && !EFI_ERROR (Status)) {\r
     return EFI_SUCCESS;\r
   }\r
 \r
+  //\r
+  // If command execution failed, then retrieve error info via sense request.\r
+  //\r
+  DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: %r to Exec 0x%x Cmd (Result = %x)\n", Status, *(UINT8 *)Cmd, CmdResult));\r
   return UsbBootRequestSense (UsbMass);\r
 }\r
 \r
 \r
 /**\r
-  Execute the USB mass storage bootability commands. If execution\r
-  failed, retrieve the error by REQUEST_SENSE then update the device's\r
-  status, such as ReadyOnly.\r
+  Execute the USB mass storage bootability commands with retrial.\r
+\r
+  This function executes USB mass storage bootability commands.\r
+  If the device isn't ready, wait for it. If the device is ready\r
+  and error occurs, retry the command again until it exceeds the\r
+  limit of retrial times.\r
 \r
   @param  UsbMass                The device to issue commands to\r
   @param  Cmd                    The command to execute\r
@@ -285,15 +225,13 @@ UsbBootExecCmd (
   @param  DataDir                The direction of data transfer\r
   @param  Data                   The buffer to hold the data\r
   @param  DataLen                The length of expected data\r
+  @param  Timeout                The timeout used to transfer\r
 \r
-  @retval EFI_SUCCESS            The command is excuted OK\r
-  @retval EFI_DEVICE_ERROR       Failed to request sense\r
-  @retval EFI_INVALID_PARAMETER  The command has some invalid parameters\r
-  @retval EFI_WRITE_PROTECTED    The device is write protected\r
-  @retval EFI_MEDIA_CHANGED      The device media has been changed\r
+  @retval EFI_SUCCESS            The command is executed successfully.\r
+  @retval EFI_NO_MEDIA           The device media is removed.\r
+  @retval Others                 Command execution failed after retrial.\r
 \r
 **/\r
-STATIC\r
 EFI_STATUS\r
 UsbBootExecCmdWithRetry (\r
   IN USB_MASS_DEVICE          *UsbMass,\r
@@ -306,18 +244,31 @@ UsbBootExecCmdWithRetry (
   )\r
 {\r
   EFI_STATUS                  Status;\r
-  INT16                       Index;\r
+  UINTN                       Retry;\r
+  EFI_EVENT                   TimeoutEvt;\r
+\r
+  Retry  = 0;\r
+  Status = EFI_SUCCESS;\r
+  Status = gBS->CreateEvent (\r
+                  EVT_TIMER,\r
+                  TPL_CALLBACK,\r
+                  NULL,\r
+                  NULL,\r
+                  &TimeoutEvt\r
+                  );\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  Status = gBS->SetTimer (TimeoutEvt, TimerRelative, EFI_TIMER_PERIOD_SECONDS(60));\r
+  if (EFI_ERROR (Status)) {\r
+    goto EXIT;\r
+  }\r
 \r
   //\r
-  // If the device isn't ready, wait some time. If the device is ready,\r
-  // retry the command again.\r
+  // Execute the cmd and retry if it fails.\r
   //\r
-  Status  = EFI_SUCCESS;\r
-\r
-  for (Index = 0; Index < USB_BOOT_COMMAND_RETRY; Index++) {\r
-    //\r
-    // Execute the command with an increasingly larger timeout value.\r
-    //\r
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvt))) {\r
     Status = UsbBootExecCmd (\r
                UsbMass,\r
                Cmd,\r
@@ -325,32 +276,41 @@ UsbBootExecCmdWithRetry (
                DataDir,\r
                Data,\r
                DataLen,\r
-               Timeout * (Index + 1)\r
+               Timeout\r
                );\r
-    if (Status == EFI_SUCCESS ||\r
-        Status == EFI_MEDIA_CHANGED) {\r
+    if (Status == EFI_SUCCESS || Status == EFI_NO_MEDIA) {\r
       break;\r
     }\r
     //\r
-    // Need retry once more, so reset index\r
+    // If the sense data shows the drive is not ready, we need execute the cmd again.\r
+    // We limit the upper boundary to 60 seconds.\r
     //\r
     if (Status == EFI_NOT_READY) {\r
-      Index = 0;\r
+      continue;\r
+    }\r
+    //\r
+    // If the status is other error, then just retry 5 times.\r
+    //\r
+    if (Retry++ >= USB_BOOT_COMMAND_RETRY) {\r
+      break;\r
     }\r
   }\r
 \r
+EXIT:\r
+  if (TimeoutEvt != NULL) {\r
+    gBS->CloseEvent (TimeoutEvt);\r
+  }\r
+\r
   return Status;\r
 }\r
 \r
 \r
-\r
 /**\r
-  Use the TEST UNIT READY command to check whether it is ready.\r
-  If it is ready, update the parameters.\r
+  Execute TEST UNIT READY command to check if the device is ready.\r
 \r
   @param  UsbMass                The device to test\r
 \r
-  @retval EFI_SUCCESS            The device is ready and parameters are updated.\r
+  @retval EFI_SUCCESS            The device is ready.\r
   @retval Others                 Device not ready.\r
 \r
 **/\r
@@ -364,12 +324,12 @@ UsbBootIsUnitReady (
   ZeroMem (&TestCmd, sizeof (USB_BOOT_TEST_UNIT_READY_CMD));\r
 \r
   TestCmd.OpCode  = USB_BOOT_TEST_UNIT_READY_OPCODE;\r
-  TestCmd.Lun     = USB_BOOT_LUN (UsbMass->Lun);\r
+  TestCmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
 \r
   return UsbBootExecCmdWithRetry (\r
            UsbMass,\r
            &TestCmd,\r
-           sizeof (USB_BOOT_TEST_UNIT_READY_CMD),\r
+           (UINT8) sizeof (USB_BOOT_TEST_UNIT_READY_CMD),\r
            EfiUsbNoData,\r
            NULL,\r
            0,\r
@@ -379,13 +339,13 @@ UsbBootIsUnitReady (
 \r
 \r
 /**\r
-  Inquiry Command requests that information regrarding parameters of\r
-  the Device be sent to the Host.\r
+  Execute INQUIRY Command to request information regarding parameters of\r
+  the device be sent to the host computer.\r
 \r
-  @param  UsbMass                The device to inquiry.\r
+  @param  UsbMass                The device to inquire.\r
 \r
-  @retval EFI_SUCCESS            The device is ready and parameters are updated.\r
-  @retval Others                 Device not ready.\r
+  @retval EFI_SUCCESS            INQUIRY Command is executed successfully.\r
+  @retval Others                 INQUIRY Command is not executed successfully.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -394,57 +354,140 @@ UsbBootInquiry (
   )\r
 {\r
   USB_BOOT_INQUIRY_CMD        InquiryCmd;\r
-  USB_BOOT_INQUIRY_DATA       InquiryData;\r
   EFI_BLOCK_IO_MEDIA          *Media;\r
   EFI_STATUS                  Status;\r
 \r
   Media = &(UsbMass->BlockIoMedia);\r
 \r
-  //\r
-  // Use the Inquiry command to get the RemovableMedia setting.\r
-  //\r
   ZeroMem (&InquiryCmd, sizeof (USB_BOOT_INQUIRY_CMD));\r
-  ZeroMem (&InquiryData, sizeof (USB_BOOT_INQUIRY_DATA));\r
+  ZeroMem (&UsbMass->InquiryData, sizeof (USB_BOOT_INQUIRY_DATA));\r
 \r
   InquiryCmd.OpCode   = USB_BOOT_INQUIRY_OPCODE;\r
-  InquiryCmd.Lun      = USB_BOOT_LUN (UsbMass->Lun);\r
-  InquiryCmd.AllocLen = sizeof (InquiryData);\r
+  InquiryCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
+  InquiryCmd.AllocLen = (UINT8) sizeof (USB_BOOT_INQUIRY_DATA);\r
 \r
   Status = UsbBootExecCmdWithRetry (\r
              UsbMass,\r
              &InquiryCmd,\r
-             sizeof (USB_BOOT_INQUIRY_CMD),\r
+             (UINT8) sizeof (USB_BOOT_INQUIRY_CMD),\r
              EfiUsbDataIn,\r
-             &InquiryData,\r
+             &UsbMass->InquiryData,\r
              sizeof (USB_BOOT_INQUIRY_DATA),\r
-             USB_BOOT_INQUIRY_CMD_TIMEOUT\r
+             USB_BOOT_GENERAL_CMD_TIMEOUT\r
              );\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
 \r
-  UsbMass->Pdt          = USB_BOOT_PDT (InquiryData.Pdt);\r
-  Media->RemovableMedia = USB_BOOT_REMOVABLE (InquiryData.Removable);\r
   //\r
-  // Default value 512 Bytes, in case no media present at first time\r
+  // Get information from PDT (Peripheral Device Type) field and Removable Medium Bit\r
+  // from the inquiry data.\r
+  //\r
+  UsbMass->Pdt          = (UINT8) (USB_BOOT_PDT (UsbMass->InquiryData.Pdt));\r
+  Media->RemovableMedia = (BOOLEAN) (USB_BOOT_REMOVABLE (UsbMass->InquiryData.Removable));\r
+  //\r
+  // Set block size to the default value of 512 Bytes, in case no media is present at first time.\r
   //\r
   Media->BlockSize      = 0x0200;\r
 \r
   return Status;\r
 }\r
 \r
+/**\r
+  Execute READ CAPACITY 16 bytes command to request information regarding\r
+  the capacity of the installed medium of the device.\r
+\r
+  This function executes READ CAPACITY 16 bytes command to get the capacity\r
+  of the USB mass storage media, including the presence, block size,\r
+  and last block number.\r
+\r
+  @param  UsbMass                The device to retireve disk gemotric.\r
+\r
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.\r
+  @retval EFI_NOT_READY          The returned block size is zero.\r
+  @retval Other                  READ CAPACITY 16 bytes command execution failed.\r
+\r
+**/\r
+EFI_STATUS\r
+UsbBootReadCapacity16 (\r
+  IN USB_MASS_DEVICE            *UsbMass\r
+  )\r
+{\r
+  UINT8                         CapacityCmd[16];\r
+  EFI_SCSI_DISK_CAPACITY_DATA16 CapacityData;\r
+  EFI_BLOCK_IO_MEDIA            *Media;\r
+  EFI_STATUS                    Status;\r
+  UINT32                        BlockSize;\r
+\r
+  Media   = &UsbMass->BlockIoMedia;\r
+\r
+  Media->MediaPresent = FALSE;\r
+  Media->LastBlock    = 0;\r
+  Media->BlockSize    = 0;\r
+\r
+  ZeroMem (CapacityCmd, sizeof (CapacityCmd));\r
+  ZeroMem (&CapacityData, sizeof (CapacityData));\r
+\r
+  CapacityCmd[0]  = EFI_SCSI_OP_READ_CAPACITY16;\r
+  CapacityCmd[1]  = 0x10;\r
+  //\r
+  // Partial medium indicator, set the bytes 2 ~ 9 of the Cdb as ZERO.\r
+  //\r
+  ZeroMem ((CapacityCmd + 2), 8);\r
+\r
+  CapacityCmd[13] = sizeof (CapacityData);\r
+\r
+  Status = UsbBootExecCmdWithRetry (\r
+             UsbMass,\r
+             CapacityCmd,\r
+             (UINT8) sizeof (CapacityCmd),\r
+             EfiUsbDataIn,\r
+             &CapacityData,\r
+             sizeof (CapacityData),\r
+             USB_BOOT_GENERAL_CMD_TIMEOUT\r
+             );\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  //\r
+  // Get the information on media presence, block size, and last block number\r
+  // from READ CAPACITY data.\r
+  //\r
+  Media->MediaPresent = TRUE;\r
+  Media->LastBlock    = SwapBytes64 (ReadUnaligned64 ((CONST UINT64 *) &(CapacityData.LastLba7)));\r
+\r
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) &(CapacityData.BlockSize3)));\r
+\r
+  Media->LowestAlignedLba = (CapacityData.LowestAlignLogic2 << 8) |\r
+                             CapacityData.LowestAlignLogic1;\r
+  Media->LogicalBlocksPerPhysicalBlock  = (1 << CapacityData.LogicPerPhysical);\r
+  if (BlockSize == 0) {\r
+    //\r
+    //  Get sense data\r
+    //\r
+    return UsbBootRequestSense (UsbMass);\r
+  } else {\r
+    Media->BlockSize = BlockSize;\r
+  }\r
+\r
+  return Status;\r
+}\r
+\r
 \r
 /**\r
-  Get the capacity of the USB mass storage media, including\r
-  the presentation, block size, and last block number. This\r
-  function is used to get the disk parameters at the start if\r
-  it is a non-removable media or to detect the media if it is\r
-  removable.\r
+  Execute READ CAPACITY command to request information regarding\r
+  the capacity of the installed medium of the device.\r
+\r
+  This function executes READ CAPACITY command to get the capacity\r
+  of the USB mass storage media, including the presence, block size,\r
+  and last block number.\r
 \r
   @param  UsbMass                The device to retireve disk gemotric.\r
 \r
-  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.\r
-  @retval EFI_DEVICE_ERROR       Something is inconsistent with the disk gemotric.\r
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.\r
+  @retval EFI_NOT_READY          The returned block size is zero.\r
+  @retval Other                  READ CAPACITY command execution failed.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -456,22 +499,20 @@ UsbBootReadCapacity (
   USB_BOOT_READ_CAPACITY_DATA CapacityData;\r
   EFI_BLOCK_IO_MEDIA          *Media;\r
   EFI_STATUS                  Status;\r
+  UINT32                      BlockSize;\r
 \r
   Media   = &UsbMass->BlockIoMedia;\r
 \r
-  //\r
-  // Use the READ CAPACITY command to get the block length and last blockno\r
-  //\r
   ZeroMem (&CapacityCmd, sizeof (USB_BOOT_READ_CAPACITY_CMD));\r
   ZeroMem (&CapacityData, sizeof (USB_BOOT_READ_CAPACITY_DATA));\r
 \r
   CapacityCmd.OpCode = USB_BOOT_READ_CAPACITY_OPCODE;\r
-  CapacityCmd.Lun    = USB_BOOT_LUN (UsbMass->Lun);\r
+  CapacityCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
 \r
   Status = UsbBootExecCmdWithRetry (\r
              UsbMass,\r
              &CapacityCmd,\r
-             sizeof (USB_BOOT_READ_CAPACITY_CMD),\r
+             (UINT8) sizeof (USB_BOOT_READ_CAPACITY_CMD),\r
              EfiUsbDataIn,\r
              &CapacityData,\r
              sizeof (USB_BOOT_READ_CAPACITY_DATA),\r
@@ -481,86 +522,101 @@ UsbBootReadCapacity (
     return Status;\r
   }\r
 \r
+  //\r
+  // Get the information on media presence, block size, and last block number\r
+  // from READ CAPACITY data.\r
+  //\r
   Media->MediaPresent = TRUE;\r
-  Media->LastBlock    = UsbBootGetUint32 (CapacityData.LastLba);\r
-  Media->BlockSize    = UsbBootGetUint32 (CapacityData.BlockLen);\r
+  Media->LastBlock    = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.LastLba));\r
 \r
-  DEBUG ((mUsbMscInfo, "UsbBootReadCapacity Success LBA=%d BlockSize=%d\n",\r
-    Media->LastBlock, Media->BlockSize));\r
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.BlockLen));\r
+  if (BlockSize == 0) {\r
+    //\r
+    //  Get sense data\r
+    //\r
+    return UsbBootRequestSense (UsbMass);\r
+  } else {\r
+    Media->BlockSize = BlockSize;\r
+  }\r
 \r
-  return EFI_SUCCESS;\r
-}\r
+  if (Media->LastBlock == 0xFFFFFFFF) {\r
+    Status = UsbBootReadCapacity16 (UsbMass);\r
+    if (!EFI_ERROR (Status)) {\r
+      UsbMass->Cdb16Byte = TRUE;\r
+    }\r
+  }\r
 \r
+  return Status;\r
+}\r
 \r
 /**\r
-  Retrieves mode sense information via sending Mode Sense\r
-  Packet Command.\r
+  Retrieves SCSI mode sense information via MODE SENSE(6) command.\r
 \r
-  @param  UsbMass                The USB_FLOPPY_DEV instance.\r
+  @param  UsbMass                The device whose sense data is requested.\r
 \r
-  @retval EFI_DEVICE_ERROR       Hardware error\r
-  @retval EFI_SUCCESS            Success\r
+  @retval EFI_SUCCESS            SCSI mode sense information retrieved successfully.\r
+  @retval Other                  Command execution failed.\r
 \r
 **/\r
 EFI_STATUS\r
-UsbBootModeSense (\r
+UsbScsiModeSense (\r
   IN USB_MASS_DEVICE          *UsbMass\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  USB_BOOT_MODE_SENSE_CMD   ModeSenseCmd;\r
-  USB_BOOT_MODE_PARA_HEADER ModeParaHeader;\r
-  UINT8                     CommandSet;\r
+  EFI_STATUS                       Status;\r
+  USB_SCSI_MODE_SENSE6_CMD         ModeSenseCmd;\r
+  USB_SCSI_MODE_SENSE6_PARA_HEADER ModeParaHeader;\r
+  EFI_BLOCK_IO_MEDIA               *Media;\r
+\r
+  Media   = &UsbMass->BlockIoMedia;\r
 \r
-  ZeroMem (&ModeSenseCmd, sizeof (USB_BOOT_MODE_SENSE_CMD));\r
-  ZeroMem (&ModeParaHeader, sizeof (USB_BOOT_MODE_PARA_HEADER));\r
+  ZeroMem (&ModeSenseCmd, sizeof (USB_SCSI_MODE_SENSE6_CMD));\r
+  ZeroMem (&ModeParaHeader, sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER));\r
 \r
   //\r
-  // overuse Context Pointer, the first field of Bot or Cbi is EFI_USB_INTERFACE_DESCRIPTOR\r
+  // MODE SENSE(6) command is defined in Section 8.2.10 of SCSI-2 Spec\r
   //\r
-  CommandSet = ((EFI_USB_INTERFACE_DESCRIPTOR *) (UsbMass->Context))->InterfaceSubClass;\r
-\r
-  if (CommandSet == USB_MASS_STORE_SCSI) {\r
-    //\r
-    // Not UFI Command Set, no ModeSense Command\r
-    //\r
-    return EFI_SUCCESS;\r
-  }\r
-\r
-  ModeSenseCmd.OpCode         = USB_BOOT_MODE_SENSE10_OPCODE;\r
-  ModeSenseCmd.PageCode       = 0x3f;\r
-  ModeSenseCmd.ParaListLenLsb = (UINT8) sizeof (USB_BOOT_MODE_PARA_HEADER);\r
+  ModeSenseCmd.OpCode         = USB_SCSI_MODE_SENSE6_OPCODE;\r
+  ModeSenseCmd.Lun            = (UINT8) USB_BOOT_LUN (UsbMass->Lun);\r
+  ModeSenseCmd.PageCode       = 0x3F;\r
+  ModeSenseCmd.AllocateLen    = (UINT8) sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER);\r
 \r
   Status = UsbBootExecCmdWithRetry (\r
              UsbMass,\r
              &ModeSenseCmd,\r
-             sizeof (USB_BOOT_MODE_SENSE_CMD),\r
+             (UINT8) sizeof (USB_SCSI_MODE_SENSE6_CMD),\r
              EfiUsbDataIn,\r
              &ModeParaHeader,\r
-             sizeof (USB_BOOT_MODE_PARA_HEADER),\r
+             sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER),\r
              USB_BOOT_GENERAL_CMD_TIMEOUT\r
              );\r
+\r
   //\r
-  // Did nothing with the Header here\r
-  // But probably should\r
+  // Format of device-specific parameter byte of the mode parameter header is defined in\r
+  // Section 8.2.10 of SCSI-2 Spec.\r
+  // BIT7 of this byte is indicates whether the medium is write protected.\r
   //\r
+  if (!EFI_ERROR (Status)) {\r
+    Media->ReadOnly = (BOOLEAN) ((ModeParaHeader.DevicePara & BIT7) != 0);\r
+  }\r
 \r
   return Status;\r
 }\r
 \r
 \r
 /**\r
-  Get the parameters for the USB mass storage media, including\r
-  the RemovableMedia, block size, and last block number. This\r
-  function is used both to initialize the media during the\r
-  DriverBindingStart and to re-initialize it when the media is\r
+  Get the parameters for the USB mass storage media.\r
+\r
+  This function get the parameters for the USB mass storage media,\r
+  It is used both to initialize the media during the Start() phase\r
+  of Driver Binding Protocol and to re-initialize it when the media is\r
   changed. Althought the RemoveableMedia is unlikely to change,\r
-  I include it here.\r
+  it is also included here.\r
 \r
-  @param  UsbMass                The device to retireve disk gemotric.\r
+  @param  UsbMass                The device to retrieve disk gemotric.\r
 \r
   @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.\r
-  @retval EFI_DEVICE_ERROR       Something is inconsistent with the disk gemotric.\r
+  @retval Other                  Failed to get the parameters.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -571,49 +627,54 @@ UsbBootGetParams (
   EFI_BLOCK_IO_MEDIA          *Media;\r
   EFI_STATUS                  Status;\r
 \r
+  Media  = &(UsbMass->BlockIoMedia);\r
+\r
   Status = UsbBootInquiry (UsbMass);\r
   if (EFI_ERROR (Status)) {\r
-    DEBUG ((mUsbMscError, "UsbBootGetParams: UsbBootInquiry (%r)\n", Status));\r
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: UsbBootInquiry (%r)\n", Status));\r
     return Status;\r
   }\r
 \r
-  Media = &(UsbMass->BlockIoMedia);\r
   //\r
-  // Don't use the Removable bit in inquirydata to test whether the media\r
+  // According to USB Mass Storage Specification for Bootability, only following\r
+  // 4 Peripheral Device Types are in spec.\r
+  //\r
+  if ((UsbMass->Pdt != USB_PDT_DIRECT_ACCESS) &&\r
+       (UsbMass->Pdt != USB_PDT_CDROM) &&\r
+       (UsbMass->Pdt != USB_PDT_OPTICAL) &&\r
+       (UsbMass->Pdt != USB_PDT_SIMPLE_DIRECT)) {\r
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: Found an unsupported peripheral type[%d]\n", UsbMass->Pdt));\r
+    return EFI_UNSUPPORTED;\r
+  }\r
+\r
+  //\r
+  // Don't use the Removable bit in inquiry data to test whether the media\r
   // is removable because many flash disks wrongly set this bit.\r
   //\r
   if ((UsbMass->Pdt == USB_PDT_CDROM) || (UsbMass->Pdt == USB_PDT_OPTICAL)) {\r
     //\r
-    // CD-Rom or Optical device\r
+    // CD-Rom device and Non-CD optical device\r
     //\r
     UsbMass->OpticalStorage = TRUE;\r
     //\r
     // Default value 2048 Bytes, in case no media present at first time\r
     //\r
     Media->BlockSize        = 0x0800;\r
-  } else {\r
-    //\r
-    // Non CD-Rom device need ModeSenseCmd between InquiryCmd and ReadCapacityCmd\r
-    //\r
-    Status = UsbBootModeSense (UsbMass);\r
-    if (EFI_ERROR (Status)) {\r
-      DEBUG ((mUsbMscError, "UsbBootGetParams: UsbBootModeSense (%r)\n", Status));\r
-      return Status;\r
-    }\r
   }\r
 \r
-  return UsbBootReadCapacity (UsbMass);\r
+  Status = UsbBootDetectMedia (UsbMass);\r
+\r
+  return Status;\r
 }\r
 \r
 \r
 /**\r
   Detect whether the removable media is present and whether it has changed.\r
-  The Non-removable media doesn't need it.\r
 \r
-  @param  UsbMass                The device to retireve disk gemotric.\r
+  @param  UsbMass                The device to check.\r
 \r
-  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.\r
-  @retval EFI_DEVICE_ERROR       Something is inconsistent with the disk gemotric.\r
+  @retval EFI_SUCCESS            The media status is successfully checked.\r
+  @retval Other                  Failed to detect media.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -623,60 +684,108 @@ UsbBootDetectMedia (
 {\r
   EFI_BLOCK_IO_MEDIA        OldMedia;\r
   EFI_BLOCK_IO_MEDIA        *Media;\r
+  UINT8                     CmdSet;\r
   EFI_STATUS                Status;\r
 \r
   Media    = &UsbMass->BlockIoMedia;\r
-  OldMedia = UsbMass->BlockIoMedia;\r
+\r
+  CopyMem (&OldMedia, &(UsbMass->BlockIoMedia), sizeof (EFI_BLOCK_IO_MEDIA));\r
+\r
+  CmdSet = ((EFI_USB_INTERFACE_DESCRIPTOR *) (UsbMass->Context))->InterfaceSubClass;\r
+\r
+  Status = UsbBootIsUnitReady (UsbMass);\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootIsUnitReady (%r)\n", Status));\r
+  }\r
 \r
   //\r
-  // First test whether the device is ready and get status\r
-  // If media changed or ready, need read the device's capacity\r
+  // Status could be:\r
+  //   EFI_SUCCESS: all good.\r
+  //   EFI_NO_MEDIA: media is not present.\r
+  //   others: HW error.\r
+  // For either EFI_NO_MEDIA, or HW error, skip to get WriteProtected and capacity information.\r
   //\r
-  Status = UsbBootIsUnitReady (UsbMass);\r
-  if ((Status == EFI_SUCCESS && Media->MediaPresent) ||\r
-      (Status == EFI_MEDIA_CHANGED)) {\r
-    if ((UsbMass->Pdt != USB_PDT_CDROM) &&\r
-        (UsbMass->Pdt != USB_PDT_OPTICAL)) {\r
+  if (!EFI_ERROR (Status)) {\r
+    if ((UsbMass->Pdt != USB_PDT_CDROM) && (CmdSet == USB_MASS_STORE_SCSI)) {\r
       //\r
-      // Non CD-Rom device need ModeSenseCmd between InquiryCmd and ReadCapacityCmd\r
+      // MODE SENSE is required for the device with PDT of 0x00/0x07/0x0E,\r
+      // according to Section 4 of USB Mass Storage Specification for Bootability.\r
+      // MODE SENSE(10) is useless here, while MODE SENSE(6) defined in SCSI\r
+      // could get the information of Write Protected.\r
+      // Since not all device support this command, skip if fail.\r
       //\r
-      UsbBootModeSense (UsbMass);\r
+      UsbScsiModeSense (UsbMass);\r
     }\r
-    DEBUG ((mUsbMscInfo, "UsbBootDetectMedia: Need Read Capacity\n"));\r
+\r
     Status = UsbBootReadCapacity (UsbMass);\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootReadCapacity (%r)\n", Status));\r
+    }\r
   }\r
-  if (EFI_ERROR (Status)) {\r
+\r
+  if (EFI_ERROR (Status) && Status != EFI_NO_MEDIA) {\r
+    //\r
+    // For NoMedia, BlockIo is still needed.\r
+    //\r
     return Status;\r
   }\r
 \r
   //\r
-  // Detect whether it is necessary to reinstall the BlockIO\r
+  // Simply reject device whose block size is unacceptable small (==0) or large (>64K).\r
+  //\r
+  if ((Media->BlockSize == 0) || (Media->BlockSize > USB_BOOT_MAX_CARRY_SIZE)) {\r
+    return EFI_DEVICE_ERROR;\r
+  }\r
+\r
+  //\r
+  // Detect whether it is necessary to reinstall the Block I/O Protocol.\r
+  //\r
+  // MediaId may change in RequestSense for MediaChanged\r
+  // MediaPresent may change in RequestSense for NoMedia\r
+  // MediaReadOnly may change in RequestSense for WriteProtected or MediaChanged\r
+  // MediaPresent/BlockSize/LastBlock may change in ReadCapacity\r
   //\r
   if ((Media->MediaId != OldMedia.MediaId) ||\r
       (Media->MediaPresent != OldMedia.MediaPresent) ||\r
       (Media->ReadOnly != OldMedia.ReadOnly) ||\r
       (Media->BlockSize != OldMedia.BlockSize) ||\r
       (Media->LastBlock != OldMedia.LastBlock)) {\r
-    DEBUG ((mUsbMscInfo, "UsbBootDetectMedia: Need reinstall BlockIoProtocol\n"));\r
-    Media->MediaId++;\r
+\r
+    //\r
+    // This function is called from:\r
+    //   Block I/O Protocol APIs, which run at TPL_CALLBACK.\r
+    //   DriverBindingStart(), which raises to TPL_CALLBACK.\r
+    ASSERT (EfiGetCurrentTpl () == TPL_CALLBACK);\r
+\r
+    //\r
+    // When it is called from DriverBindingStart(), below reinstall fails.\r
+    // So ignore the return status check.\r
+    //\r
     gBS->ReinstallProtocolInterface (\r
            UsbMass->Controller,\r
            &gEfiBlockIoProtocolGuid,\r
            &UsbMass->BlockIo,\r
            &UsbMass->BlockIo\r
            );\r
+\r
     //\r
-    // Check whether media present or media changed or write protected\r
+    // Reset MediaId after reinstalling Block I/O Protocol.\r
     //\r
-    if (Media->MediaPresent == FALSE) {\r
-      Status = EFI_NO_MEDIA;\r
-    }\r
-    if (Media->MediaId != OldMedia.MediaId) {\r
-      Status = EFI_MEDIA_CHANGED;\r
+    if (Media->MediaPresent != OldMedia.MediaPresent) {\r
+      if (Media->MediaPresent) {\r
+        Media->MediaId = 1;\r
+      } else {\r
+        Media->MediaId = 0;\r
+      }\r
     }\r
-    if (Media->ReadOnly != OldMedia.ReadOnly) {\r
-      Status = EFI_WRITE_PROTECTED;\r
+\r
+    if ((Media->ReadOnly != OldMedia.ReadOnly) ||\r
+        (Media->BlockSize != OldMedia.BlockSize) ||\r
+        (Media->LastBlock != OldMedia.LastBlock)) {\r
+      Media->MediaId++;\r
     }\r
+\r
+    Status = Media->MediaPresent ? EFI_MEDIA_CHANGED : EFI_NO_MEDIA;\r
   }\r
 \r
   return Status;\r
@@ -684,33 +793,37 @@ UsbBootDetectMedia (
 \r
 \r
 /**\r
-  Read some blocks from the device.\r
+  Read or write some blocks from the device.\r
 \r
-  @param  UsbMass                The USB mass storage device to read from\r
+  @param  UsbMass                The USB mass storage device to access\r
+  @param  Write                  TRUE for write operation.\r
   @param  Lba                    The start block number\r
-  @param  TotalBlock             Total block number to read\r
-  @param  Buffer                 The buffer to read to\r
+  @param  TotalBlock             Total block number to read or write\r
+  @param  Buffer                 The buffer to read to or write from\r
 \r
-  @retval EFI_SUCCESS            Data are read into the buffer\r
-  @retval Others                 Failed to read all the data\r
+  @retval EFI_SUCCESS            Data are read into the buffer or writen into the device.\r
+  @retval Others                 Failed to read or write all the data\r
 \r
 **/\r
 EFI_STATUS\r
-UsbBootReadBlocks (\r
+UsbBootReadWriteBlocks (\r
   IN  USB_MASS_DEVICE       *UsbMass,\r
+  IN  BOOLEAN               Write,\r
   IN  UINT32                Lba,\r
   IN  UINTN                 TotalBlock,\r
-  OUT UINT8                 *Buffer\r
+  IN OUT UINT8              *Buffer\r
   )\r
 {\r
-  USB_BOOT_READ10_CMD       ReadCmd;\r
-  EFI_STATUS                Status;\r
-  UINT16                    Count;\r
-  UINT32                    BlockSize;\r
-  UINT32                    ByteSize;\r
-  UINT32                    Timeout;\r
+  USB_BOOT_READ_WRITE_10_CMD Cmd;\r
+  EFI_STATUS                 Status;\r
+  UINT32                     Count;\r
+  UINT32                     CountMax;\r
+  UINT32                     BlockSize;\r
+  UINT32                     ByteSize;\r
+  UINT32                     Timeout;\r
 \r
   BlockSize = UsbMass->BlockIoMedia.BlockSize;\r
+  CountMax  = USB_BOOT_MAX_CARRY_SIZE / BlockSize;\r
   Status    = EFI_SUCCESS;\r
 \r
   while (TotalBlock > 0) {\r
@@ -719,33 +832,30 @@ UsbBootReadBlocks (
     // on the device. We must split the total block because the READ10\r
     // command only has 16 bit transfer length (in the unit of block).\r
     //\r
-    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);\r
-    ByteSize  = (UINT32)Count * BlockSize;\r
+    Count    = (UINT32)MIN (TotalBlock, CountMax);\r
+    Count    = MIN (MAX_UINT16, Count);\r
+    ByteSize = Count * BlockSize;\r
 \r
     //\r
-    // Optical device need longer timeout than other device\r
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
     //\r
-    if (UsbMass->OpticalStorage == TRUE) {\r
-      Timeout = (UINT32)Count * USB_BOOT_OPTICAL_BLOCK_TIMEOUT;\r
-    } else {\r
-      Timeout = (UINT32)Count * USB_BOOT_GENERAL_BLOCK_TIMEOUT;\r
-    }\r
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;\r
 \r
     //\r
     // Fill in the command then execute\r
     //\r
-    ZeroMem (&ReadCmd, sizeof (USB_BOOT_READ10_CMD));\r
+    ZeroMem (&Cmd, sizeof (USB_BOOT_READ_WRITE_10_CMD));\r
 \r
-    ReadCmd.OpCode  = USB_BOOT_READ10_OPCODE;\r
-    ReadCmd.Lun     = USB_BOOT_LUN (UsbMass->Lun);\r
-    UsbBootPutUint32 (ReadCmd.Lba, Lba);\r
-    UsbBootPutUint16 (ReadCmd.TransferLen, Count);\r
+    Cmd.OpCode  = Write ? USB_BOOT_WRITE10_OPCODE : USB_BOOT_READ10_OPCODE;\r
+    Cmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
+    WriteUnaligned32 ((UINT32 *) Cmd.Lba, SwapBytes32 (Lba));\r
+    WriteUnaligned16 ((UINT16 *) Cmd.TransferLen, SwapBytes16 ((UINT16)Count));\r
 \r
     Status = UsbBootExecCmdWithRetry (\r
                UsbMass,\r
-               &ReadCmd,\r
-               sizeof (USB_BOOT_READ10_CMD),\r
-               EfiUsbDataIn,\r
+               &Cmd,\r
+               (UINT8) sizeof (USB_BOOT_READ_WRITE_10_CMD),\r
+               Write ? EfiUsbDataOut : EfiUsbDataIn,\r
                Buffer,\r
                ByteSize,\r
                Timeout\r
@@ -753,79 +863,79 @@ UsbBootReadBlocks (
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-\r
+    DEBUG ((\r
+      DEBUG_BLKIO, "UsbBoot%sBlocks: LBA (0x%lx), Blk (0x%x)\n",\r
+      Write ? L"Write" : L"Read",\r
+      Lba, Count\r
+      ));\r
     Lba        += Count;\r
-    Buffer     += Count * BlockSize;\r
+    Buffer     += ByteSize;\r
     TotalBlock -= Count;\r
   }\r
 \r
   return Status;\r
 }\r
 \r
-\r
 /**\r
-  Write some blocks to the device.\r
+  Read or write some blocks from the device by SCSI 16 byte cmd.\r
 \r
-  @param  UsbMass                The USB mass storage device to write to\r
+  @param  UsbMass                The USB mass storage device to access\r
+  @param  Write                  TRUE for write operation.\r
   @param  Lba                    The start block number\r
-  @param  TotalBlock             Total block number to write\r
-  @param  Buffer                 The buffer to write to\r
-\r
-  @retval EFI_SUCCESS            Data are written into the buffer\r
-  @retval Others                 Failed to write all the data\r
+  @param  TotalBlock             Total block number to read or write\r
+  @param  Buffer                 The buffer to read to or write from\r
 \r
+  @retval EFI_SUCCESS            Data are read into the buffer or writen into the device.\r
+  @retval Others                 Failed to read or write all the data\r
 **/\r
 EFI_STATUS\r
-UsbBootWriteBlocks (\r
-  IN  USB_MASS_DEVICE         *UsbMass,\r
-  IN  UINT32                  Lba,\r
-  IN  UINTN                   TotalBlock,\r
-  OUT UINT8                   *Buffer\r
+UsbBootReadWriteBlocks16 (\r
+  IN  USB_MASS_DEVICE       *UsbMass,\r
+  IN  BOOLEAN               Write,\r
+  IN  UINT64                Lba,\r
+  IN  UINTN                 TotalBlock,\r
+  IN OUT UINT8              *Buffer\r
   )\r
 {\r
-  USB_BOOT_WRITE10_CMD  WriteCmd;\r
-  EFI_STATUS            Status;\r
-  UINT16                Count;\r
-  UINT32                BlockSize;\r
-  UINT32                ByteSize;\r
-  UINT32                Timeout;\r
+  UINT8                     Cmd[16];\r
+  EFI_STATUS                Status;\r
+  UINT32                    Count;\r
+  UINT32                    CountMax;\r
+  UINT32                    BlockSize;\r
+  UINT32                    ByteSize;\r
+  UINT32                    Timeout;\r
 \r
   BlockSize = UsbMass->BlockIoMedia.BlockSize;\r
+  CountMax  = USB_BOOT_MAX_CARRY_SIZE / BlockSize;\r
   Status    = EFI_SUCCESS;\r
 \r
   while (TotalBlock > 0) {\r
     //\r
-    // Split the total blocks into smaller pieces to ease the pressure\r
-    // on the device. We must split the total block because the WRITE10\r
-    // command only has 16 bit transfer length (in the unit of block).\r
+    // Split the total blocks into smaller pieces.\r
     //\r
-    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);\r
-    ByteSize  = (UINT32)Count * BlockSize;\r
+    Count    = (UINT32)MIN (TotalBlock, CountMax);\r
+    ByteSize Count * BlockSize;\r
 \r
     //\r
-    // Optical device need longer timeout than other device\r
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
     //\r
-    if (UsbMass->OpticalStorage == TRUE) {\r
-      Timeout = (UINT32)Count * USB_BOOT_OPTICAL_BLOCK_TIMEOUT;\r
-    } else {\r
-      Timeout = (UINT32)Count * USB_BOOT_GENERAL_BLOCK_TIMEOUT;\r
-    }\r
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;\r
 \r
     //\r
-    // Fill in the write10 command block\r
+    // Fill in the command then execute\r
     //\r
-    ZeroMem (&WriteCmd, sizeof (USB_BOOT_WRITE10_CMD));\r
+    ZeroMem (Cmd, sizeof (Cmd));\r
 \r
-    WriteCmd.OpCode = USB_BOOT_WRITE10_OPCODE;\r
-    WriteCmd.Lun    = USB_BOOT_LUN (UsbMass->Lun);\r
-    UsbBootPutUint32 (WriteCmd.Lba, Lba);\r
-    UsbBootPutUint16 (WriteCmd.TransferLen, Count);\r
+    Cmd[0]  = Write ? EFI_SCSI_OP_WRITE16 : EFI_SCSI_OP_READ16;\r
+    Cmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));\r
+    WriteUnaligned64 ((UINT64 *) &Cmd[2], SwapBytes64 (Lba));\r
+    WriteUnaligned32 ((UINT32 *) &Cmd[10], SwapBytes32 (Count));\r
 \r
     Status = UsbBootExecCmdWithRetry (\r
                UsbMass,\r
-               &WriteCmd,\r
-               sizeof (USB_BOOT_WRITE10_CMD),\r
-               EfiUsbDataOut,\r
+               Cmd,\r
+               (UINT8) sizeof (Cmd),\r
+               Write ? EfiUsbDataOut : EfiUsbDataIn,\r
                Buffer,\r
                ByteSize,\r
                Timeout\r
@@ -833,25 +943,27 @@ UsbBootWriteBlocks (
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-\r
+    DEBUG ((\r
+      DEBUG_BLKIO, "UsbBoot%sBlocks16: LBA (0x%lx), Blk (0x%x)\n",\r
+      Write ? L"Write" : L"Read",\r
+      Lba, Count\r
+      ));\r
     Lba        += Count;\r
-    Buffer     += Count * BlockSize;\r
+    Buffer     += ByteSize;\r
     TotalBlock -= Count;\r
   }\r
 \r
   return Status;\r
 }\r
 \r
-\r
 /**\r
-  Use the USB clear feature control transfer to clear the endpoint\r
-  stall condition.\r
+  Use the USB clear feature control transfer to clear the endpoint stall condition.\r
 \r
-  @param  UsbIo                  The USB IO protocol to use\r
+  @param  UsbIo                  The USB I/O Protocol instance\r
   @param  EndpointAddr           The endpoint to clear stall for\r
 \r
-  @retval EFI_SUCCESS            The endpoint stall condtion is clear\r
-  @retval Others                 Failed to clear the endpoint stall condtion\r
+  @retval EFI_SUCCESS            The endpoint stall condition is cleared.\r
+  @retval Others                 Failed to clear the endpoint stall condition.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -870,7 +982,7 @@ UsbClearEndpointStall (
   Request.Value       = USB_FEATURE_ENDPOINT_HALT;\r
   Request.Index       = EndpointAddr;\r
   Request.Length      = 0;\r
-  Timeout             = USB_BOOT_GENERAL_CMD_TIMEOUT / USB_MASS_STALL_1_MS;\r
+  Timeout             = USB_BOOT_GENERAL_CMD_TIMEOUT / USB_MASS_1_MILLISECOND;\r
 \r
   Status = UsbIo->UsbControlTransfer (\r
                     UsbIo,\r