]> 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 a8b6a1c5f13c9cca81490650ddb6c0a5ca013fa5..600896b6a2147c6c5cc17356a842b44347000434 100644 (file)
@@ -2,14 +2,8 @@
   Implementation of the command set of USB Mass Storage Specification\r
   for Bootability, Revision 1.0.\r
 \r
-Copyright (c) 2007 - 2017, Intel Corporation. All rights reserved.<BR>\r
-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
+Copyright (c) 2007 - 2018, Intel Corporation. All rights reserved.<BR>\r
+SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
@@ -137,8 +131,9 @@ UsbBootRequestSense (
     break;\r
   }\r
 \r
-  DEBUG ((EFI_D_INFO, "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
@@ -196,7 +191,7 @@ UsbBootExecCmd (
                            );\r
 \r
   if (Status == EFI_TIMEOUT) {\r
-    DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: Timeout to Exec 0x%x Cmd\n", *(UINT8 *)Cmd));\r
+    DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: %r to Exec 0x%x Cmd\n", Status, *(UINT8 *)Cmd));\r
     return EFI_TIMEOUT;\r
   }\r
 \r
@@ -211,6 +206,7 @@ UsbBootExecCmd (
   //\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
@@ -222,7 +218,7 @@ UsbBootExecCmd (
   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
+\r
   @param  UsbMass                The device to issue commands to\r
   @param  Cmd                    The command to execute\r
   @param  CmdLen                 The length of the command\r
@@ -232,7 +228,7 @@ UsbBootExecCmd (
   @param  Timeout                The timeout used to transfer\r
 \r
   @retval EFI_SUCCESS            The command is executed successfully.\r
-  @retval EFI_MEDIA_CHANGED      The device media has been changed.\r
+  @retval EFI_NO_MEDIA           The device media is removed.\r
   @retval Others                 Command execution failed after retrial.\r
 \r
 **/\r
@@ -282,7 +278,7 @@ UsbBootExecCmdWithRetry (
                DataLen,\r
                Timeout\r
                );\r
-    if (Status == EFI_SUCCESS || Status == EFI_MEDIA_CHANGED || Status == EFI_NO_MEDIA) {\r
+    if (Status == EFI_SUCCESS || Status == EFI_NO_MEDIA) {\r
       break;\r
     }\r
     //\r
@@ -410,7 +406,7 @@ UsbBootInquiry (
   @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
 **/\r
 EFI_STATUS\r
 UsbBootReadCapacity16 (\r
@@ -440,7 +436,7 @@ UsbBootReadCapacity16 (
   ZeroMem ((CapacityCmd + 2), 8);\r
 \r
   CapacityCmd[13] = sizeof (CapacityData);\r
-  \r
+\r
   Status = UsbBootExecCmdWithRetry (\r
              UsbMass,\r
              CapacityCmd,\r
@@ -462,13 +458,13 @@ UsbBootReadCapacity16 (
   Media->LastBlock    = SwapBytes64 (ReadUnaligned64 ((CONST UINT64 *) &(CapacityData.LastLba7)));\r
 \r
   BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) &(CapacityData.BlockSize3)));\r
-  \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
+    //  Get sense data\r
     //\r
     return UsbBootRequestSense (UsbMass);\r
   } else {\r
@@ -492,7 +488,7 @@ UsbBootReadCapacity16 (
   @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
 **/\r
 EFI_STATUS\r
 UsbBootReadCapacity (\r
@@ -536,7 +532,7 @@ UsbBootReadCapacity (
   BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.BlockLen));\r
   if (BlockSize == 0) {\r
     //\r
-    //  Get sense data  \r
+    //  Get sense data\r
     //\r
     return UsbBootRequestSense (UsbMass);\r
   } else {\r
@@ -689,7 +685,6 @@ UsbBootDetectMedia (
   EFI_BLOCK_IO_MEDIA        OldMedia;\r
   EFI_BLOCK_IO_MEDIA        *Media;\r
   UINT8                     CmdSet;\r
-  EFI_TPL                   OldTpl;\r
   EFI_STATUS                Status;\r
 \r
   Media    = &UsbMass->BlockIoMedia;\r
@@ -699,30 +694,49 @@ UsbBootDetectMedia (
   CmdSet = ((EFI_USB_INTERFACE_DESCRIPTOR *) (UsbMass->Context))->InterfaceSubClass;\r
 \r
   Status = UsbBootIsUnitReady (UsbMass);\r
-  if (EFI_ERROR (Status) && (Status != EFI_MEDIA_CHANGED)) {\r
-    goto ON_ERROR;\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootIsUnitReady (%r)\n", Status));\r
+  }\r
+\r
+  //\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
+  if (!EFI_ERROR (Status)) {\r
+    if ((UsbMass->Pdt != USB_PDT_CDROM) && (CmdSet == USB_MASS_STORE_SCSI)) {\r
+      //\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
+      UsbScsiModeSense (UsbMass);\r
+    }\r
+\r
+    Status = UsbBootReadCapacity (UsbMass);\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootReadCapacity (%r)\n", Status));\r
+    }\r
   }\r
 \r
-  if ((UsbMass->Pdt != USB_PDT_CDROM) && (CmdSet == USB_MASS_STORE_SCSI)) {\r
+  if (EFI_ERROR (Status) && Status != EFI_NO_MEDIA) {\r
     //\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
+    // For NoMedia, BlockIo is still needed.\r
     //\r
-    UsbScsiModeSense (UsbMass);\r
+    return Status;\r
   }\r
 \r
-  Status = UsbBootReadCapacity (UsbMass);\r
-  if (EFI_ERROR (Status)) {\r
-    DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootReadCapacity (%r)\n", Status));\r
-    goto ON_ERROR;\r
+  //\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
-  return EFI_SUCCESS;\r
-\r
-ON_ERROR:\r
   //\r
   // Detect whether it is necessary to reinstall the Block I/O Protocol.\r
   //\r
@@ -738,12 +752,15 @@ ON_ERROR:
       (Media->LastBlock != OldMedia.LastBlock)) {\r
 \r
     //\r
-    // This function is called by Block I/O Protocol APIs, which run at TPL_NOTIFY.\r
-    // Here we temporarily restore TPL to TPL_CALLBACK to invoke ReinstallProtocolInterface().\r
-    //\r
-    OldTpl = EfiGetCurrentTpl ();\r
-    gBS->RestoreTPL (TPL_CALLBACK);\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
@@ -751,11 +768,8 @@ ON_ERROR:
            &UsbMass->BlockIo\r
            );\r
 \r
-    ASSERT (EfiGetCurrentTpl () == TPL_CALLBACK);\r
-    gBS->RaiseTPL (OldTpl);\r
-\r
     //\r
-    // Update MediaId after reinstalling Block I/O Protocol.\r
+    // Reset MediaId after reinstalling Block I/O Protocol.\r
     //\r
     if (Media->MediaPresent != OldMedia.MediaPresent) {\r
       if (Media->MediaPresent) {\r
@@ -770,6 +784,8 @@ ON_ERROR:
         (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
@@ -777,33 +793,37 @@ ON_ERROR:
 \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
@@ -812,8 +832,9 @@ 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
     // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
@@ -823,18 +844,18 @@ UsbBootReadBlocks (
     //\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     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
-    WriteUnaligned32 ((UINT32 *) ReadCmd.Lba, SwapBytes32 (Lba));\r
-    WriteUnaligned16 ((UINT16 *) ReadCmd.TransferLen, SwapBytes16 (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
-               (UINT8) 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
@@ -842,128 +863,58 @@ UsbBootReadBlocks (
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));\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                 Pointer to the source buffer for the data.\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
-  IN  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
-\r
-  BlockSize = UsbMass->BlockIoMedia.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
-    //\r
-    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);\r
-    ByteSize  = (UINT32)Count * BlockSize;\r
-\r
-    //\r
-    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
-    //\r
-    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;\r
-\r
-    //\r
-    // Fill in the write10 command block\r
-    //\r
-    ZeroMem (&WriteCmd, sizeof (USB_BOOT_WRITE10_CMD));\r
-\r
-    WriteCmd.OpCode = USB_BOOT_WRITE10_OPCODE;\r
-    WriteCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));\r
-    WriteUnaligned32 ((UINT32 *) WriteCmd.Lba, SwapBytes32 (Lba));\r
-    WriteUnaligned16 ((UINT16 *) WriteCmd.TransferLen, SwapBytes16 (Count));\r
-\r
-    Status = UsbBootExecCmdWithRetry (\r
-               UsbMass,\r
-               &WriteCmd,\r
-               (UINT8) sizeof (USB_BOOT_WRITE10_CMD),\r
-               EfiUsbDataOut,\r
-               Buffer,\r
-               ByteSize,\r
-               Timeout\r
-               );\r
-    if (EFI_ERROR (Status)) {\r
-      return Status;\r
-    }\r
-    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));\r
-\r
-    Lba        += Count;\r
-    Buffer     += Count * BlockSize;\r
-    TotalBlock -= Count;\r
-  }\r
-\r
-  return Status;\r
-}\r
-\r
-/**\r
-  Read some blocks from the device by SCSI 16 byte cmd.\r
-\r
-  @param  UsbMass                The USB mass storage device to read from\r
-  @param  Lba                    The start block number\r
-  @param  TotalBlock             Total block number to read\r
-  @param  Buffer                 The buffer to read to\r
-\r
-  @retval EFI_SUCCESS            Data are read into the buffer\r
-  @retval Others                 Failed to read all the data\r
-\r
-**/\r
-EFI_STATUS\r
-UsbBootReadBlocks16 (\r
+UsbBootReadWriteBlocks16 (\r
   IN  USB_MASS_DEVICE       *UsbMass,\r
+  IN  BOOLEAN               Write,\r
   IN  UINT64                Lba,\r
   IN  UINTN                 TotalBlock,\r
-  OUT UINT8                 *Buffer\r
+  IN OUT UINT8              *Buffer\r
   )\r
 {\r
-  UINT8                     ReadCmd[16];\r
+  UINT8                     Cmd[16];\r
   EFI_STATUS                Status;\r
-  UINT16                    Count;\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.\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
     // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
@@ -973,92 +924,18 @@ UsbBootReadBlocks16 (
     //\r
     // Fill in the command then execute\r
     //\r
-    ZeroMem (ReadCmd, sizeof (ReadCmd));\r
+    ZeroMem (Cmd, sizeof (Cmd));\r
 \r
-    ReadCmd[0]  = EFI_SCSI_OP_READ16;\r
-    ReadCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));\r
-    WriteUnaligned64 ((UINT64 *) &ReadCmd[2], SwapBytes64 (Lba));\r
-    WriteUnaligned32 ((UINT32 *) &ReadCmd[10], SwapBytes32 (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
-               ReadCmd,\r
-               (UINT8) sizeof (ReadCmd),\r
-               EfiUsbDataIn,\r
-               Buffer,\r
-               ByteSize,\r
-               Timeout\r
-               );\r
-    if (EFI_ERROR (Status)) {\r
-      return Status;\r
-    }\r
-    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks16: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));\r
-    Lba        += Count;\r
-    Buffer     += Count * BlockSize;\r
-    TotalBlock -= Count;\r
-  }\r
-\r
-  return Status;\r
-}\r
-\r
-\r
-/**\r
-  Write some blocks to the device by SCSI 16 byte cmd.\r
-\r
-  @param  UsbMass                The USB mass storage device to write to\r
-  @param  Lba                    The start block number\r
-  @param  TotalBlock             Total block number to write\r
-  @param  Buffer                 Pointer to the source buffer for the data.\r
-\r
-  @retval EFI_SUCCESS            Data are written into the buffer\r
-  @retval Others                 Failed to write all the data\r
-\r
-**/\r
-EFI_STATUS\r
-UsbBootWriteBlocks16 (\r
-  IN  USB_MASS_DEVICE         *UsbMass,\r
-  IN  UINT64                  Lba,\r
-  IN  UINTN                   TotalBlock,\r
-  IN  UINT8                   *Buffer\r
-  )\r
-{\r
-  UINT8                 WriteCmd[16];\r
-  EFI_STATUS            Status;\r
-  UINT16                Count;\r
-  UINT32                BlockSize;\r
-  UINT32                ByteSize;\r
-  UINT32                Timeout;\r
-\r
-  BlockSize = UsbMass->BlockIoMedia.BlockSize;\r
-  Status    = EFI_SUCCESS;\r
-\r
-  while (TotalBlock > 0) {\r
-    //\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
-\r
-    //\r
-    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]\r
-    //\r
-    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;\r
-\r
-    //\r
-    // Fill in the write16 command block\r
-    //\r
-    ZeroMem (WriteCmd, sizeof (WriteCmd));\r
-\r
-    WriteCmd[0]  = EFI_SCSI_OP_WRITE16;\r
-    WriteCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));\r
-    WriteUnaligned64 ((UINT64 *) &WriteCmd[2], SwapBytes64 (Lba));\r
-    WriteUnaligned32 ((UINT32 *) &WriteCmd[10], SwapBytes32 (Count));\r
-\r
-    Status = UsbBootExecCmdWithRetry (\r
-               UsbMass,\r
-               WriteCmd,\r
-               (UINT8) sizeof (WriteCmd),\r
-               EfiUsbDataOut,\r
+               Cmd,\r
+               (UINT8) sizeof (Cmd),\r
+               Write ? EfiUsbDataOut : EfiUsbDataIn,\r
                Buffer,\r
                ByteSize,\r
                Timeout\r
@@ -1066,9 +943,13 @@ UsbBootWriteBlocks16 (
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));\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