Remove Debug message and early test code in DiskIo
authorlgao4 <lgao4@6f19259b-4bc3-4df7-8a09-765794883524>
Fri, 2 Jun 2006 09:42:14 +0000 (09:42 +0000)
committerlgao4 <lgao4@6f19259b-4bc3-4df7-8a09-765794883524>
Fri, 2 Jun 2006 09:42:14 +0000 (09:42 +0000)
git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@396 6f19259b-4bc3-4df7-8a09-765794883524

EdkModulePkg/Universal/Disk/DiskIo/Dxe/diskio.c

index 4998c9b..35d2a19 100644 (file)
-/*++\r
-\r
-Copyright (c) 2006, 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
-  DiskIo.c\r
-\r
-Abstract:\r
-\r
-  DiskIo driver that layers it's self on every Block IO protocol in the system.\r
-  DiskIo converts a block oriented device to a byte oriented device.\r
-\r
-  ReadDisk may have to do reads that are not aligned on sector boundaries.\r
-  There are three cases:\r
-\r
-    UnderRun - The first byte is not on a sector boundary or the read request is\r
-               less than a sector in length.\r
-\r
-    Aligned  - A read of N contiguous sectors.\r
-\r
-    OverRun  - The last byte is not on a sector boundary.\r
-\r
---*/\r
-\r
-#include "DiskIo.h"\r
-\r
-//\r
-// Prototypes\r
-// Driver model protocol interface\r
-//\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingSupported (\r
-  IN EFI_DRIVER_BINDING_PROTOCOL    *This,\r
-  IN EFI_HANDLE                     ControllerHandle,\r
-  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath\r
-  );\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingStart (\r
-  IN EFI_DRIVER_BINDING_PROTOCOL    *This,\r
-  IN EFI_HANDLE                     ControllerHandle,\r
-  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath\r
-  );\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingStop (\r
-  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,\r
-  IN  EFI_HANDLE                     ControllerHandle,\r
-  IN  UINTN                          NumberOfChildren,\r
-  IN  EFI_HANDLE                     *ChildHandleBuffer\r
-  );\r
-\r
-//\r
-// Disk I/O Protocol Interface\r
-//\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoReadDisk (\r
-  IN EFI_DISK_IO_PROTOCOL  *This,\r
-  IN UINT32                MediaId,\r
-  IN UINT64                Offset,\r
-  IN UINTN                 BufferSize,\r
-  OUT VOID                 *Buffer\r
-  );\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoWriteDisk (\r
-  IN EFI_DISK_IO_PROTOCOL  *This,\r
-  IN UINT32                MediaId,\r
-  IN UINT64                Offset,\r
-  IN UINTN                 BufferSize,\r
-  IN VOID                  *Buffer\r
-  );\r
-\r
-EFI_DRIVER_BINDING_PROTOCOL gDiskIoDriverBinding = {\r
-  DiskIoDriverBindingSupported,\r
-  DiskIoDriverBindingStart,\r
-  DiskIoDriverBindingStop,\r
-  0x10,\r
-  NULL,\r
-  NULL\r
-};\r
-\r
-DISK_IO_PRIVATE_DATA        gDiskIoPrivateDataTemplate = {\r
-  DISK_IO_PRIVATE_DATA_SIGNATURE,\r
-  {\r
-    EFI_DISK_IO_PROTOCOL_REVISION,\r
-    DiskIoReadDisk,\r
-    DiskIoWriteDisk\r
-  },\r
-  NULL\r
-};\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingSupported (\r
-  IN EFI_DRIVER_BINDING_PROTOCOL  *This,\r
-  IN EFI_HANDLE                   ControllerHandle,\r
-  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL\r
-  )\r
-/*++\r
-\r
-  Routine Description:\r
-    Test to see if this driver supports ControllerHandle. Any ControllerHandle\r
-    than contains a BlockIo protocol can be supported.\r
-\r
-  Arguments:\r
-    This                - Protocol instance pointer.\r
-    ControllerHandle    - Handle of device to test.\r
-    RemainingDevicePath - Not used.\r
-\r
-  Returns:\r
-    EFI_SUCCESS         - This driver supports this device.\r
-    EFI_ALREADY_STARTED - This driver is already running on this device.\r
-    other               - This driver does not support this device.\r
-\r
---*/\r
-{\r
-  EFI_STATUS            Status;\r
-  EFI_BLOCK_IO_PROTOCOL *BlockIo;\r
-/*\r
-  DEBUG_CODE_BEGIN\r
-    UINT32  Bar;\r
-    UINT32  Foo;\r
-    UINT32  HotPlug;\r
-\r
-    //\r
-    // Get TYPE 0\r
-    //\r
-    Bar = PcdGet32 (PciExpressBaseVersion);\r
-    DEBUG ((EFI_D_ERROR, "PciExpressBaseVersion = %08x\n", Bar));\r
-\r
-    //\r
-    // Get TYPE 1\r
-    //\r
-    Foo = PcdGet32 (PciExpressBaseAddress);\r
-    DEBUG ((EFI_D_ERROR, "PciExpressBaseAddress = %08x\n", Foo));\r
-\r
-    //\r
-    // Set TYPE 1\r
-    //\r
-    PcdSet32 (PciExpressBaseAddress, Foo + 1);\r
-\r
-    //\r
-    // Get TYPE 1\r
-    //\r
-    Foo = PcdGet32 (PciExpressBaseAddress);\r
-    DEBUG ((EFI_D_ERROR, "PciExpressBaseAddress = %08x\n", Foo));\r
-\r
-    //\r
-    // Get TYPE 2\r
-    //\r
-    HotPlug = PcdGet32 (PciExpressBaseHotPlug);\r
-    DEBUG ((EFI_D_ERROR, "PciExpressHotPlug = %08x\n", HotPlug));\r
-\r
-    //\r
-    // Set TYPE 1\r
-    //\r
-    PcdSet32 (PciExpressBaseHotPlug, HotPlug + 1);\r
-\r
-    //\r
-    // Get TYPE 1\r
-    //\r
-    HotPlug = PcdGet32 (PciExpressBaseHotPlug);\r
-    DEBUG ((EFI_D_ERROR, "PciExpressHotPlug = %08x\n", HotPlug));\r
-\r
-  DEBUG_CODE_END\r
-\r
-  DEBUG_CODE_BEGIN\r
-    UINT32  MyVariable;\r
-\r
-    if (ControllerHandle == NULL) {\r
-      MyVariable = 32 * (UINTN)This;\r
-      ControllerHandle = (EFI_HANDLE)MyVariable;\r
-      DEBUG ((EFI_D_ERROR, "DiskIoSupported-DebugCode.  MyVariable = %08x\n", MyVariable));\r
-      ASSERT (MyVariable != 32);\r
-    }\r
-  DEBUG_CODE_END\r
-*/\r
-  DEBUG ((EFI_D_ERROR, "DiskIoSupported\n"));\r
-\r
-//  Io8Or (0x400, 1);\r
-//  Io8And (0x400, 1);\r
-//  Io8AndThenOr (0x400, 1, 2);\r
-\r
-//  Mmio8Or (0xa0000000, 1);\r
-//  Mmio8And (0xa0000000, 1);\r
-//  Mmio8AndThenOr (0xa0000000, 1, 2);\r
-\r
-/*\r
-  PciRead8   (PCI_LIB_ADDRESS (1,2,3,4));\r
-  PciRead16  (PCI_LIB_ADDRESS (1,2,3,4));\r
-  PciRead32  (PCI_LIB_ADDRESS (1,2,3,4));\r
-\r
-  PciWrite8  (PCI_LIB_ADDRESS (1,2,3,4), 0xAA);\r
-  PciWrite16 (PCI_LIB_ADDRESS (1,2,3,4), 0xAA55);\r
-  PciWrite32 (PCI_LIB_ADDRESS (1,2,3,4), 0xAA55A55A);\r
-\r
-  Pci8Or         (PCI_LIB_ADDRESS (1,2,3,4), 0xAA);\r
-  Pci8And        (PCI_LIB_ADDRESS (1,2,3,4), 0x55);\r
-  Pci8AndThenOr  (PCI_LIB_ADDRESS (1,2,3,4), 0xAA, 0x55);\r
-\r
-  Pci16Or        (PCI_LIB_ADDRESS (1,2,3,4), 0xAA55);\r
-  Pci16And       (PCI_LIB_ADDRESS (1,2,3,4), 0x55AA);\r
-  Pci16AndThenOr (PCI_LIB_ADDRESS (1,2,3,4), 0xAA55, 0x55AA);\r
-\r
-  Pci32Or        (PCI_LIB_ADDRESS (1,2,3,4), 0xAA55A55A);\r
-  Pci32And       (PCI_LIB_ADDRESS (1,2,3,4), 0x55AA5AA5);\r
-  Pci32AndThenOr (PCI_LIB_ADDRESS (1,2,3,4), 0xAA555AA5, 0x55AAA55A);\r
-*/\r
-  //\r
-  // Open the IO Abstraction(s) needed to perform the supported test.\r
-  //\r
-  Status = gBS->OpenProtocol (\r
-                  ControllerHandle,\r
-                  &gEfiBlockIoProtocolGuid,\r
-                  (VOID **) &BlockIo,\r
-                  This->DriverBindingHandle,\r
-                  ControllerHandle,\r
-                  EFI_OPEN_PROTOCOL_BY_DRIVER\r
-                  );\r
-  if (EFI_ERROR (Status)) {\r
-    return Status;\r
-  }\r
-  //\r
-  // Close the I/O Abstraction(s) used to perform the supported test.\r
-  //\r
-  gBS->CloseProtocol (\r
-        ControllerHandle,\r
-        &gEfiBlockIoProtocolGuid,\r
-        This->DriverBindingHandle,\r
-        ControllerHandle\r
-        );\r
-  return EFI_SUCCESS;\r
-}\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingStart (\r
-  IN EFI_DRIVER_BINDING_PROTOCOL  *This,\r
-  IN EFI_HANDLE                   ControllerHandle,\r
-  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL\r
-  )\r
-/*++\r
-\r
-  Routine Description:\r
-    Start this driver on ControllerHandle by opening a Block IO protocol and\r
-    installing a Disk IO protocol on ControllerHandle.\r
-\r
-  Arguments:\r
-    This                - Protocol instance pointer.\r
-    ControllerHandle    - Handle of device to bind driver to.\r
-    RemainingDevicePath - Not used, always produce all possible children.\r
-\r
-  Returns:\r
-    EFI_SUCCESS         - This driver is added to ControllerHandle.\r
-    EFI_ALREADY_STARTED - This driver is already running on ControllerHandle.\r
-    other               - This driver does not support this device.\r
-\r
---*/\r
-{\r
-  EFI_STATUS            Status;\r
-  DISK_IO_PRIVATE_DATA  *Private;\r
-\r
-  Private = NULL;\r
-\r
-  DEBUG ((EFI_D_ERROR, "DiskIoStart\n"));\r
-  //\r
-  // Connect to the Block IO interface on ControllerHandle.\r
-  //\r
-  Status = gBS->OpenProtocol (\r
-                  ControllerHandle,\r
-                  &gEfiBlockIoProtocolGuid,\r
-                  (VOID **) &gDiskIoPrivateDataTemplate.BlockIo,\r
-                  This->DriverBindingHandle,\r
-                  ControllerHandle,\r
-                  EFI_OPEN_PROTOCOL_BY_DRIVER\r
-                  );\r
-  if (EFI_ERROR (Status)) {\r
-    return Status;\r
-  }\r
-  //\r
-  // Initialize the Disk IO device instance.\r
-  //\r
-  Private = AllocateCopyPool (sizeof (DISK_IO_PRIVATE_DATA), &gDiskIoPrivateDataTemplate);\r
-  if (Private == NULL) {\r
-    Status = EFI_OUT_OF_RESOURCES;\r
-    goto ErrorExit;\r
-  }\r
-  //\r
-  // Install protocol interfaces for the Disk IO device.\r
-  //\r
-  Status = gBS->InstallProtocolInterface (\r
-                  &ControllerHandle,\r
-                  &gEfiDiskIoProtocolGuid,\r
-                  EFI_NATIVE_INTERFACE,\r
-                  &Private->DiskIo\r
-                  );\r
-\r
-ErrorExit:\r
-  if (EFI_ERROR (Status)) {\r
-\r
-    if (Private != NULL) {\r
-      gBS->FreePool (Private);\r
-    }\r
-\r
-    gBS->CloseProtocol (\r
-          ControllerHandle,\r
-          &gEfiBlockIoProtocolGuid,\r
-          This->DriverBindingHandle,\r
-          ControllerHandle\r
-          );\r
-  }\r
-\r
-  return Status;\r
-}\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoDriverBindingStop (\r
-  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,\r
-  IN  EFI_HANDLE                     ControllerHandle,\r
-  IN  UINTN                          NumberOfChildren,\r
-  IN  EFI_HANDLE                     *ChildHandleBuffer\r
-  )\r
-/*++\r
-\r
-  Routine Description:\r
-    Stop this driver on ControllerHandle by removing Disk IO protocol and closing\r
-    the Block IO protocol on ControllerHandle.\r
-\r
-  Arguments:\r
-    This              - Protocol instance pointer.\r
-    ControllerHandle  - Handle of device to stop driver on.\r
-    NumberOfChildren  - Not used.\r
-    ChildHandleBuffer - Not used.\r
-\r
-  Returns:\r
-    EFI_SUCCESS         - This driver is removed ControllerHandle.\r
-    other               - This driver was not removed from this device.\r
-    EFI_UNSUPPORTED\r
-\r
---*/\r
-{\r
-  EFI_STATUS            Status;\r
-  EFI_DISK_IO_PROTOCOL  *DiskIo;\r
-  DISK_IO_PRIVATE_DATA  *Private;\r
-\r
-  DEBUG ((EFI_D_ERROR, "DiskIoStop\n"));\r
-  //\r
-  // Get our context back.\r
-  //\r
-  Status = gBS->OpenProtocol (\r
-                  ControllerHandle,\r
-                  &gEfiDiskIoProtocolGuid,\r
-                  (VOID **) &DiskIo,\r
-                  This->DriverBindingHandle,\r
-                  ControllerHandle,\r
-                  EFI_OPEN_PROTOCOL_GET_PROTOCOL\r
-                  );\r
-  if (EFI_ERROR (Status)) {\r
-    return EFI_UNSUPPORTED;\r
-  }\r
-\r
-  Private = DISK_IO_PRIVATE_DATA_FROM_THIS (DiskIo);\r
-\r
-  Status = gBS->UninstallProtocolInterface (\r
-                  ControllerHandle,\r
-                  &gEfiDiskIoProtocolGuid,\r
-                  &Private->DiskIo\r
-                  );\r
-  if (!EFI_ERROR (Status)) {\r
-\r
-    Status = gBS->CloseProtocol (\r
-                    ControllerHandle,\r
-                    &gEfiBlockIoProtocolGuid,\r
-                    This->DriverBindingHandle,\r
-                    ControllerHandle\r
-                    );\r
-  }\r
-\r
-  if (!EFI_ERROR (Status)) {\r
-    gBS->FreePool (Private);\r
-  }\r
-\r
-  return Status;\r
-}\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoReadDisk (\r
-  IN EFI_DISK_IO_PROTOCOL  *This,\r
-  IN UINT32                MediaId,\r
-  IN UINT64                Offset,\r
-  IN UINTN                 BufferSize,\r
-  OUT VOID                 *Buffer\r
-  )\r
-/*++\r
-\r
-  Routine Description:\r
-    Read BufferSize bytes from Offset into Buffer.\r
-\r
-    Reads may support reads that are not aligned on\r
-    sector boundaries. There are three cases:\r
-\r
-      UnderRun - The first byte is not on a sector boundary or the read request is\r
-                 less than a sector in length.\r
-\r
-      Aligned  - A read of N contiguous sectors.\r
-\r
-      OverRun  - The last byte is not on a sector boundary.\r
-\r
-\r
-  Arguments:\r
-    This       - Protocol instance pointer.\r
-    MediaId    - Id of the media, changes every time the media is replaced.\r
-    Offset     - The starting byte offset to read from.\r
-    BufferSize - Size of Buffer.\r
-    Buffer     - Buffer containing read data.\r
-\r
-  Returns:\r
-    EFI_SUCCESS           - The data was read correctly from the device.\r
-    EFI_DEVICE_ERROR      - The device reported an error while performing the read.\r
-    EFI_NO_MEDIA          - There is no media in the device.\r
-    EFI_MEDIA_CHNAGED     - The MediaId does not matched the current device.\r
-    EFI_INVALID_PARAMETER - The read request contains device addresses that are not\r
-                            valid for the device.\r
-    EFI_OUT_OF_RESOURCES\r
-\r
---*/\r
-{\r
-  EFI_STATUS            Status;\r
-  DISK_IO_PRIVATE_DATA  *Private;\r
-  EFI_BLOCK_IO_PROTOCOL *BlockIo;\r
-  EFI_BLOCK_IO_MEDIA    *Media;\r
-  UINT32                BlockSize;\r
-  UINT64                Lba;\r
-  UINT64                OverRunLba;\r
-  UINT32                UnderRun;\r
-  UINT32                OverRun;\r
-  BOOLEAN               TransactionComplete;\r
-  UINTN                 WorkingBufferSize;\r
-  UINT8                 *WorkingBuffer;\r
-  UINTN                 Length;\r
-  UINT8                 *Data;\r
-  UINT8                 *PreData;\r
-  UINTN                 IsBufferAligned;\r
-  UINTN                 DataBufferSize;\r
-  BOOLEAN               LastRead;\r
-\r
-  DEBUG ((EFI_D_ERROR, "DiskIoReadDisk\n"));\r
-\r
-  Private   = DISK_IO_PRIVATE_DATA_FROM_THIS (This);\r
-\r
-  BlockIo   = Private->BlockIo;\r
-  Media     = BlockIo->Media;\r
-  BlockSize = Media->BlockSize;\r
-\r
-  if (Media->MediaId != MediaId) {\r
-    return EFI_MEDIA_CHANGED;\r
-  }\r
-\r
-  WorkingBuffer     = Buffer;\r
-  WorkingBufferSize = BufferSize;\r
-\r
-  //\r
-  // Allocate a temporary buffer for operation\r
-  //\r
-  DataBufferSize = BlockSize * DATA_BUFFER_BLOCK_NUM;\r
-\r
-  if (Media->IoAlign > 1) {\r
-    PreData = AllocatePool (DataBufferSize + Media->IoAlign);\r
-    Data    = PreData - ((UINTN) PreData & (Media->IoAlign - 1)) + Media->IoAlign;\r
-  } else {\r
-    PreData = AllocatePool (DataBufferSize);\r
-    Data    = PreData;\r
-  }\r
-\r
-  if (PreData == NULL) {\r
-    return EFI_OUT_OF_RESOURCES;\r
-  }\r
-\r
-  Lba                 = DivU64x32Remainder (Offset, BlockSize, &UnderRun);\r
-\r
-  Length              = BlockSize - UnderRun;\r
-  TransactionComplete = FALSE;\r
-\r
-  Status              = EFI_SUCCESS;\r
-  if (UnderRun != 0) {\r
-    //\r
-    // Offset starts in the middle of an Lba, so read the entire block.\r
-    //\r
-    Status = BlockIo->ReadBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        Lba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-\r
-    if (Length > BufferSize) {\r
-      Length              = BufferSize;\r
-      TransactionComplete = TRUE;\r
-    }\r
-\r
-    CopyMem (WorkingBuffer, Data + UnderRun, Length);\r
-\r
-    WorkingBuffer += Length;\r
-\r
-    WorkingBufferSize -= Length;\r
-    if (WorkingBufferSize == 0) {\r
-      goto Done;\r
-    }\r
-\r
-    Lba += 1;\r
-  }\r
-\r
-  OverRunLba = Lba + DivU64x32Remainder (WorkingBufferSize, BlockSize, &OverRun);\r
-\r
-  if (!TransactionComplete && WorkingBufferSize >= BlockSize) {\r
-    //\r
-    // If the DiskIo maps directly to a BlockIo device do the read.\r
-    //\r
-    if (OverRun != 0) {\r
-      WorkingBufferSize -= OverRun;\r
-    }\r
-    //\r
-    // Check buffer alignment\r
-    //\r
-    IsBufferAligned = (UINTN) WorkingBuffer & (UINTN) (Media->IoAlign - 1);\r
-\r
-    if (Media->IoAlign <= 1 || IsBufferAligned == 0) {\r
-      //\r
-      // Alignment is satisfied, so read them together\r
-      //\r
-      Status = BlockIo->ReadBlocks (\r
-                          BlockIo,\r
-                          MediaId,\r
-                          Lba,\r
-                          WorkingBufferSize,\r
-                          WorkingBuffer\r
-                          );\r
-\r
-      if (EFI_ERROR (Status)) {\r
-        goto Done;\r
-      }\r
-\r
-      WorkingBuffer += WorkingBufferSize;\r
-\r
-    } else {\r
-      //\r
-      // Use the allocated buffer instead of the original buffer\r
-      // to avoid alignment issue.\r
-      // Here, the allocated buffer (8-byte align) can satisfy the alignment\r
-      //\r
-      LastRead = FALSE;\r
-      do {\r
-        if (WorkingBufferSize <= DataBufferSize) {\r
-          //\r
-          // It is the last calling to readblocks in this loop\r
-          //\r
-          DataBufferSize  = WorkingBufferSize;\r
-          LastRead        = TRUE;\r
-        }\r
-\r
-        Status = BlockIo->ReadBlocks (\r
-                            BlockIo,\r
-                            MediaId,\r
-                            Lba,\r
-                            DataBufferSize,\r
-                            Data\r
-                            );\r
-        if (EFI_ERROR (Status)) {\r
-          goto Done;\r
-        }\r
-\r
-        CopyMem (WorkingBuffer, Data, DataBufferSize);\r
-        WorkingBufferSize -= DataBufferSize;\r
-        WorkingBuffer += DataBufferSize;\r
-        Lba += DATA_BUFFER_BLOCK_NUM;\r
-      } while (!LastRead);\r
-    }\r
-  }\r
-\r
-  if (!TransactionComplete && OverRun != 0) {\r
-    //\r
-    // Last read is not a complete block.\r
-    //\r
-    Status = BlockIo->ReadBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        OverRunLba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-\r
-    CopyMem (WorkingBuffer, Data, OverRun);\r
-  }\r
-\r
-Done:\r
-  if (PreData != NULL) {\r
-    gBS->FreePool (PreData);\r
-  }\r
-\r
-  return Status;\r
-}\r
-\r
-EFI_STATUS\r
-EFIAPI\r
-DiskIoWriteDisk (\r
-  IN EFI_DISK_IO_PROTOCOL  *This,\r
-  IN UINT32                MediaId,\r
-  IN UINT64                Offset,\r
-  IN UINTN                 BufferSize,\r
-  IN VOID                  *Buffer\r
-  )\r
-/*++\r
-\r
-  Routine Description:\r
-    Read BufferSize bytes from Offset into Buffer.\r
-\r
-    Writes may require a read modify write to support writes that are not\r
-    aligned on sector boundaries. There are three cases:\r
-\r
-      UnderRun - The first byte is not on a sector boundary or the write request\r
-                 is less than a sector in length. Read modify write is required.\r
-\r
-      Aligned  - A write of N contiguous sectors.\r
-\r
-      OverRun  - The last byte is not on a sector boundary. Read modified write\r
-                 required.\r
-\r
-  Arguments:\r
-    This       - Protocol instance pointer.\r
-    MediaId    - Id of the media, changes every time the media is replaced.\r
-    Offset     - The starting byte offset to read from.\r
-    BufferSize - Size of Buffer.\r
-    Buffer     - Buffer containing read data.\r
-\r
-  Returns:\r
-    EFI_SUCCESS           - The data was written correctly to the device.\r
-    EFI_WRITE_PROTECTED   - The device can not be written to.\r
-    EFI_DEVICE_ERROR      - The device reported an error while performing the write.\r
-    EFI_NO_MEDIA          - There is no media in the device.\r
-    EFI_MEDIA_CHNAGED     - The MediaId does not matched the current device.\r
-    EFI_INVALID_PARAMETER - The write request contains device addresses that are not\r
-                            valid for the device.\r
-    EFI_OUT_OF_RESOURCES\r
-\r
---*/\r
-{\r
-  EFI_STATUS            Status;\r
-  DISK_IO_PRIVATE_DATA  *Private;\r
-  EFI_BLOCK_IO_PROTOCOL *BlockIo;\r
-  EFI_BLOCK_IO_MEDIA    *Media;\r
-  UINT32                BlockSize;\r
-  UINT64                Lba;\r
-  UINT64                OverRunLba;\r
-  UINT32                UnderRun;\r
-  UINT32                OverRun;\r
-  BOOLEAN               TransactionComplete;\r
-  UINTN                 WorkingBufferSize;\r
-  UINT8                 *WorkingBuffer;\r
-  UINTN                 Length;\r
-  UINT8                 *Data;\r
-  UINT8                 *PreData;\r
-  UINTN                 IsBufferAligned;\r
-  UINTN                 DataBufferSize;\r
-  BOOLEAN               LastWrite;\r
-\r
-  DEBUG ((EFI_D_ERROR, "DiskIoWriteDisk\n"));\r
-\r
-  Private   = DISK_IO_PRIVATE_DATA_FROM_THIS (This);\r
-\r
-  BlockIo   = Private->BlockIo;\r
-  Media     = BlockIo->Media;\r
-  BlockSize = Media->BlockSize;\r
-\r
-  if (Media->ReadOnly) {\r
-    return EFI_WRITE_PROTECTED;\r
-  }\r
-\r
-  if (Media->MediaId != MediaId) {\r
-    return EFI_MEDIA_CHANGED;\r
-  }\r
-\r
-  DataBufferSize = BlockSize * DATA_BUFFER_BLOCK_NUM;\r
-\r
-  if (Media->IoAlign > 1) {\r
-    PreData = AllocatePool (DataBufferSize + Media->IoAlign);\r
-    Data    = PreData - ((UINTN) PreData & (Media->IoAlign - 1)) + Media->IoAlign;\r
-  } else {\r
-    PreData = AllocatePool (DataBufferSize);\r
-    Data    = PreData;\r
-  }\r
-\r
-  if (PreData == NULL) {\r
-    return EFI_OUT_OF_RESOURCES;\r
-  }\r
-\r
-  WorkingBuffer       = Buffer;\r
-  WorkingBufferSize   = BufferSize;\r
-\r
-  Lba                 = DivU64x32Remainder (Offset, BlockSize, &UnderRun);\r
-\r
-  Length              = BlockSize - UnderRun;\r
-  TransactionComplete = FALSE;\r
-\r
-  Status              = EFI_SUCCESS;\r
-  if (UnderRun != 0) {\r
-    //\r
-    // Offset starts in the middle of an Lba, so do read modify write.\r
-    //\r
-    Status = BlockIo->ReadBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        Lba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-\r
-    if (Length > BufferSize) {\r
-      Length              = BufferSize;\r
-      TransactionComplete = TRUE;\r
-    }\r
-\r
-    CopyMem (Data + UnderRun, WorkingBuffer, Length);\r
-\r
-    Status = BlockIo->WriteBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        Lba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-\r
-    WorkingBuffer += Length;\r
-    WorkingBufferSize -= Length;\r
-    if (WorkingBufferSize == 0) {\r
-      goto Done;\r
-    }\r
-\r
-    Lba += 1;\r
-  }\r
-\r
-  OverRunLba = Lba + DivU64x32Remainder (WorkingBufferSize, BlockSize, &OverRun);\r
-\r
-  if (!TransactionComplete && WorkingBufferSize >= BlockSize) {\r
-    //\r
-    // If the DiskIo maps directly to a BlockIo device do the write.\r
-    //\r
-    if (OverRun != 0) {\r
-      WorkingBufferSize -= OverRun;\r
-    }\r
-    //\r
-    // Check buffer alignment\r
-    //\r
-    IsBufferAligned = (UINTN) WorkingBuffer & (UINTN) (Media->IoAlign - 1);\r
-\r
-    if (Media->IoAlign <= 1 || IsBufferAligned == 0) {\r
-      //\r
-      // Alignment is satisfied, so write them together\r
-      //\r
-      Status = BlockIo->WriteBlocks (\r
-                          BlockIo,\r
-                          MediaId,\r
-                          Lba,\r
-                          WorkingBufferSize,\r
-                          WorkingBuffer\r
-                          );\r
-\r
-      if (EFI_ERROR (Status)) {\r
-        goto Done;\r
-      }\r
-\r
-      WorkingBuffer += WorkingBufferSize;\r
-\r
-    } else {\r
-      //\r
-      // The buffer parameter is not aligned with the request\r
-      // So use the allocated instead.\r
-      // It can fit almost all the cases.\r
-      //\r
-      LastWrite = FALSE;\r
-      do {\r
-        if (WorkingBufferSize <= DataBufferSize) {\r
-          //\r
-          // It is the last calling to writeblocks in this loop\r
-          //\r
-          DataBufferSize  = WorkingBufferSize;\r
-          LastWrite       = TRUE;\r
-        }\r
-\r
-        CopyMem (Data, WorkingBuffer, DataBufferSize);\r
-        Status = BlockIo->WriteBlocks (\r
-                            BlockIo,\r
-                            MediaId,\r
-                            Lba,\r
-                            DataBufferSize,\r
-                            Data\r
-                            );\r
-        if (EFI_ERROR (Status)) {\r
-          goto Done;\r
-        }\r
-\r
-        WorkingBufferSize -= DataBufferSize;\r
-        WorkingBuffer += DataBufferSize;\r
-        Lba += DATA_BUFFER_BLOCK_NUM;\r
-      } while (!LastWrite);\r
-    }\r
-  }\r
-\r
-  if (!TransactionComplete && OverRun != 0) {\r
-    //\r
-    // Last bit is not a complete block, so do a read modify write.\r
-    //\r
-    Status = BlockIo->ReadBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        OverRunLba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-\r
-    CopyMem (Data, WorkingBuffer, OverRun);\r
-\r
-    Status = BlockIo->WriteBlocks (\r
-                        BlockIo,\r
-                        MediaId,\r
-                        OverRunLba,\r
-                        BlockSize,\r
-                        Data\r
-                        );\r
-    if (EFI_ERROR (Status)) {\r
-      goto Done;\r
-    }\r
-  }\r
-\r
-Done:\r
-  if (PreData != NULL) {\r
-    gBS->FreePool (PreData);\r
-  }\r
-\r
-  return Status;\r
-}\r
+/*++
+
+Copyright (c) 2006, Intel Corporation
+All rights reserved. This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+Module Name:
+
+  DiskIo.c
+
+Abstract:
+
+  DiskIo driver that layers it's self on every Block IO protocol in the system.
+  DiskIo converts a block oriented device to a byte oriented device.
+
+  ReadDisk may have to do reads that are not aligned on sector boundaries.
+  There are three cases:
+
+    UnderRun - The first byte is not on a sector boundary or the read request is
+               less than a sector in length.
+
+    Aligned  - A read of N contiguous sectors.
+
+    OverRun  - The last byte is not on a sector boundary.
+
+--*/
+
+#include "DiskIo.h"
+
+//
+// Prototypes
+// Driver model protocol interface
+//
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     ControllerHandle,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+//
+// Disk I/O Protocol Interface
+//
+EFI_STATUS
+EFIAPI
+DiskIoReadDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  OUT VOID                 *Buffer
+  );
+
+EFI_STATUS
+EFIAPI
+DiskIoWriteDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  IN VOID                  *Buffer
+  );
+
+EFI_DRIVER_BINDING_PROTOCOL gDiskIoDriverBinding = {
+  DiskIoDriverBindingSupported,
+  DiskIoDriverBindingStart,
+  DiskIoDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+DISK_IO_PRIVATE_DATA        gDiskIoPrivateDataTemplate = {
+  DISK_IO_PRIVATE_DATA_SIGNATURE,
+  {
+    EFI_DISK_IO_PROTOCOL_REVISION,
+    DiskIoReadDisk,
+    DiskIoWriteDisk
+  },
+  NULL
+};
+
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+/*++
+
+  Routine Description:
+    Test to see if this driver supports ControllerHandle. Any ControllerHandle
+    than contains a BlockIo protocol can be supported.
+
+  Arguments:
+    This                - Protocol instance pointer.
+    ControllerHandle    - Handle of device to test.
+    RemainingDevicePath - Not used.
+
+  Returns:
+    EFI_SUCCESS         - This driver supports this device.
+    EFI_ALREADY_STARTED - This driver is already running on this device.
+    other               - This driver does not support this device.
+
+--*/
+{
+  EFI_STATUS            Status;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test.
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        &gEfiBlockIoProtocolGuid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+/*++
+
+  Routine Description:
+    Start this driver on ControllerHandle by opening a Block IO protocol and
+    installing a Disk IO protocol on ControllerHandle.
+
+  Arguments:
+    This                - Protocol instance pointer.
+    ControllerHandle    - Handle of device to bind driver to.
+    RemainingDevicePath - Not used, always produce all possible children.
+
+  Returns:
+    EFI_SUCCESS         - This driver is added to ControllerHandle.
+    EFI_ALREADY_STARTED - This driver is already running on ControllerHandle.
+    other               - This driver does not support this device.
+
+--*/
+{
+  EFI_STATUS            Status;
+  DISK_IO_PRIVATE_DATA  *Private;
+
+  Private = NULL;
+
+  //
+  // Connect to the Block IO interface on ControllerHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &gDiskIoPrivateDataTemplate.BlockIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Initialize the Disk IO device instance.
+  //
+  Private = AllocateCopyPool (sizeof (DISK_IO_PRIVATE_DATA), &gDiskIoPrivateDataTemplate);
+  if (Private == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+  //
+  // Install protocol interfaces for the Disk IO device.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Private->DiskIo
+                  );
+
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+
+    if (Private != NULL) {
+      gBS->FreePool (Private);
+    }
+
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiBlockIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     ControllerHandle,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+/*++
+
+  Routine Description:
+    Stop this driver on ControllerHandle by removing Disk IO protocol and closing
+    the Block IO protocol on ControllerHandle.
+
+  Arguments:
+    This              - Protocol instance pointer.
+    ControllerHandle  - Handle of device to stop driver on.
+    NumberOfChildren  - Not used.
+    ChildHandleBuffer - Not used.
+
+  Returns:
+    EFI_SUCCESS         - This driver is removed ControllerHandle.
+    other               - This driver was not removed from this device.
+    EFI_UNSUPPORTED
+
+--*/
+{
+  EFI_STATUS            Status;
+  EFI_DISK_IO_PROTOCOL  *DiskIo;
+  DISK_IO_PRIVATE_DATA  *Private;
+
+  //
+  // Get our context back.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  (VOID **) &DiskIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Private = DISK_IO_PRIVATE_DATA_FROM_THIS (DiskIo);
+
+  Status = gBS->UninstallProtocolInterface (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  &Private->DiskIo
+                  );
+  if (!EFI_ERROR (Status)) {
+
+    Status = gBS->CloseProtocol (
+                    ControllerHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    This->DriverBindingHandle,
+                    ControllerHandle
+                    );
+  }
+
+  if (!EFI_ERROR (Status)) {
+    gBS->FreePool (Private);
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+EFIAPI
+DiskIoReadDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  OUT VOID                 *Buffer
+  )
+/*++
+
+  Routine Description:
+    Read BufferSize bytes from Offset into Buffer.
+
+    Reads may support reads that are not aligned on
+    sector boundaries. There are three cases:
+
+      UnderRun - The first byte is not on a sector boundary or the read request is
+                 less than a sector in length.
+
+      Aligned  - A read of N contiguous sectors.
+
+      OverRun  - The last byte is not on a sector boundary.
+
+
+  Arguments:
+    This       - Protocol instance pointer.
+    MediaId    - Id of the media, changes every time the media is replaced.
+    Offset     - The starting byte offset to read from.
+    BufferSize - Size of Buffer.
+    Buffer     - Buffer containing read data.
+
+  Returns:
+    EFI_SUCCESS           - The data was read correctly from the device.
+    EFI_DEVICE_ERROR      - The device reported an error while performing the read.
+    EFI_NO_MEDIA          - There is no media in the device.
+    EFI_MEDIA_CHNAGED     - The MediaId does not matched the current device.
+    EFI_INVALID_PARAMETER - The read request contains device addresses that are not
+                            valid for the device.
+    EFI_OUT_OF_RESOURCES
+
+--*/
+{
+  EFI_STATUS            Status;
+  DISK_IO_PRIVATE_DATA  *Private;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+  EFI_BLOCK_IO_MEDIA    *Media;
+  UINT32                BlockSize;
+  UINT64                Lba;
+  UINT64                OverRunLba;
+  UINT32                UnderRun;
+  UINT32                OverRun;
+  BOOLEAN               TransactionComplete;
+  UINTN                 WorkingBufferSize;
+  UINT8                 *WorkingBuffer;
+  UINTN                 Length;
+  UINT8                 *Data;
+  UINT8                 *PreData;
+  UINTN                 IsBufferAligned;
+  UINTN                 DataBufferSize;
+  BOOLEAN               LastRead;
+
+  Private   = DISK_IO_PRIVATE_DATA_FROM_THIS (This);
+
+  BlockIo   = Private->BlockIo;
+  Media     = BlockIo->Media;
+  BlockSize = Media->BlockSize;
+
+  if (Media->MediaId != MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  WorkingBuffer     = Buffer;
+  WorkingBufferSize = BufferSize;
+
+  //
+  // Allocate a temporary buffer for operation
+  //
+  DataBufferSize = BlockSize * DATA_BUFFER_BLOCK_NUM;
+
+  if (Media->IoAlign > 1) {
+    PreData = AllocatePool (DataBufferSize + Media->IoAlign);
+    Data    = PreData - ((UINTN) PreData & (Media->IoAlign - 1)) + Media->IoAlign;
+  } else {
+    PreData = AllocatePool (DataBufferSize);
+    Data    = PreData;
+  }
+
+  if (PreData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Lba                 = DivU64x32Remainder (Offset, BlockSize, &UnderRun);
+
+  Length              = BlockSize - UnderRun;
+  TransactionComplete = FALSE;
+
+  Status              = EFI_SUCCESS;
+  if (UnderRun != 0) {
+    //
+    // Offset starts in the middle of an Lba, so read the entire block.
+    //
+    Status = BlockIo->ReadBlocks (
+                        BlockIo,
+                        MediaId,
+                        Lba,
+                        BlockSize,
+                        Data
+                        );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    if (Length > BufferSize) {
+      Length              = BufferSize;
+      TransactionComplete = TRUE;
+    }
+
+    CopyMem (WorkingBuffer, Data + UnderRun, Length);
+
+    WorkingBuffer += Length;
+
+    WorkingBufferSize -= Length;
+    if (WorkingBufferSize == 0) {
+      goto Done;
+    }
+
+    Lba += 1;
+  }
+
+  OverRunLba = Lba + DivU64x32Remainder (WorkingBufferSize, BlockSize, &OverRun);
+
+  if (!TransactionComplete && WorkingBufferSize >= BlockSize) {
+    //
+    // If the DiskIo maps directly to a BlockIo device do the read.
+    //
+    if (OverRun != 0) {
+      WorkingBufferSize -= OverRun;
+    }
+    //
+    // Check buffer alignment
+    //
+    IsBufferAligned = (UINTN) WorkingBuffer & (UINTN) (Media->IoAlign - 1);
+
+    if (Media->IoAlign <= 1 || IsBufferAligned == 0) {
+      //
+      // Alignment is satisfied, so read them together
+      //
+      Status = BlockIo->ReadBlocks (
+                          BlockIo,
+                          MediaId,
+                          Lba,
+                          WorkingBufferSize,
+                          WorkingBuffer
+                          );
+
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      WorkingBuffer += WorkingBufferSize;
+
+    } else {
+      //
+      // Use the allocated buffer instead of the original buffer
+      // to avoid alignment issue.
+      // Here, the allocated buffer (8-byte align) can satisfy the alignment
+      //
+      LastRead = FALSE;
+      do {
+        if (WorkingBufferSize <= DataBufferSize) {
+          //
+          // It is the last calling to readblocks in this loop
+          //
+          DataBufferSize  = WorkingBufferSize;
+          LastRead        = TRUE;
+        }
+
+        Status = BlockIo->ReadBlocks (
+                            BlockIo,
+                            MediaId,
+                            Lba,
+                            DataBufferSize,
+                            Data
+                            );
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+
+        CopyMem (WorkingBuffer, Data, DataBufferSize);
+        WorkingBufferSize -= DataBufferSize;
+        WorkingBuffer += DataBufferSize;
+        Lba += DATA_BUFFER_BLOCK_NUM;
+      } while (!LastRead);
+    }
+  }
+
+  if (!TransactionComplete && OverRun != 0) {
+    //
+    // Last read is not a complete block.
+    //
+    Status = BlockIo->ReadBlocks (
+                        BlockIo,
+                        MediaId,
+                        OverRunLba,
+                        BlockSize,
+                        Data
+                        );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    CopyMem (WorkingBuffer, Data, OverRun);
+  }
+
+Done:
+  if (PreData != NULL) {
+    gBS->FreePool (PreData);
+  }
+
+  return Status;
+}
+
+EFI_STATUS
+EFIAPI
+DiskIoWriteDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  IN VOID                  *Buffer
+  )
+/*++
+
+  Routine Description:
+    Read BufferSize bytes from Offset into Buffer.
+
+    Writes may require a read modify write to support writes that are not
+    aligned on sector boundaries. There are three cases:
+
+      UnderRun - The first byte is not on a sector boundary or the write request
+                 is less than a sector in length. Read modify write is required.
+
+      Aligned  - A write of N contiguous sectors.
+
+      OverRun  - The last byte is not on a sector boundary. Read modified write
+                 required.
+
+  Arguments:
+    This       - Protocol instance pointer.
+    MediaId    - Id of the media, changes every time the media is replaced.
+    Offset     - The starting byte offset to read from.
+    BufferSize - Size of Buffer.
+    Buffer     - Buffer containing read data.
+
+  Returns:
+    EFI_SUCCESS           - The data was written correctly to the device.
+    EFI_WRITE_PROTECTED   - The device can not be written to.
+    EFI_DEVICE_ERROR      - The device reported an error while performing the write.
+    EFI_NO_MEDIA          - There is no media in the device.
+    EFI_MEDIA_CHNAGED     - The MediaId does not matched the current device.
+    EFI_INVALID_PARAMETER - The write request contains device addresses that are not
+                            valid for the device.
+    EFI_OUT_OF_RESOURCES
+
+--*/
+{
+  EFI_STATUS            Status;
+  DISK_IO_PRIVATE_DATA  *Private;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+  EFI_BLOCK_IO_MEDIA    *Media;
+  UINT32                BlockSize;
+  UINT64                Lba;
+  UINT64                OverRunLba;
+  UINT32                UnderRun;
+  UINT32                OverRun;
+  BOOLEAN               TransactionComplete;
+  UINTN                 WorkingBufferSize;
+  UINT8                 *WorkingBuffer;
+  UINTN                 Length;
+  UINT8                 *Data;
+  UINT8                 *PreData;
+  UINTN                 IsBufferAligned;
+  UINTN                 DataBufferSize;
+  BOOLEAN               LastWrite;
+
+  Private   = DISK_IO_PRIVATE_DATA_FROM_THIS (This);
+
+  BlockIo   = Private->BlockIo;
+  Media     = BlockIo->Media;
+  BlockSize = Media->BlockSize;
+
+  if (Media->ReadOnly) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  if (Media->MediaId != MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  DataBufferSize = BlockSize * DATA_BUFFER_BLOCK_NUM;
+
+  if (Media->IoAlign > 1) {
+    PreData = AllocatePool (DataBufferSize + Media->IoAlign);
+    Data    = PreData - ((UINTN) PreData & (Media->IoAlign - 1)) + Media->IoAlign;
+  } else {
+    PreData = AllocatePool (DataBufferSize);
+    Data    = PreData;
+  }
+
+  if (PreData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  WorkingBuffer       = Buffer;
+  WorkingBufferSize   = BufferSize;
+
+  Lba                 = DivU64x32Remainder (Offset, BlockSize, &UnderRun);
+
+  Length              = BlockSize - UnderRun;
+  TransactionComplete = FALSE;
+
+  Status              = EFI_SUCCESS;
+  if (UnderRun != 0) {
+    //
+    // Offset starts in the middle of an Lba, so do read modify write.
+    //
+    Status = BlockIo->ReadBlocks (
+                        BlockIo,
+                        MediaId,
+                        Lba,
+                        BlockSize,
+                        Data
+                        );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    if (Length > BufferSize) {
+      Length              = BufferSize;
+      TransactionComplete = TRUE;
+    }
+
+    CopyMem (Data + UnderRun, WorkingBuffer, Length);
+
+    Status = BlockIo->WriteBlocks (
+                        BlockIo,
+                        MediaId,
+                        Lba,
+                        BlockSize,
+                        Data
+                        );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    WorkingBuffer += Length;
+    WorkingBufferSize -= Length;
+    if (WorkingBufferSize == 0) {
+      goto Done;
+    }
+
+    Lba += 1;
+  }
+
+  OverRunLba = Lba + DivU64x32Remainder (WorkingBufferSize, BlockSize, &OverRun);
+
+  if (!TransactionComplete && WorkingBufferSize >= BlockSize) {
+    //
+    // If the DiskIo maps directly to a BlockIo device do the write.
+    //
+    if (OverRun != 0) {
+      WorkingBufferSize -= OverRun;
+    }
+    //
+    // Check buffer alignment
+    //
+    IsBufferAligned = (UINTN) WorkingBuffer & (UINTN) (Media->IoAlign - 1);
+
+    if (Media->IoAlign <= 1 || IsBufferAligned == 0) {
+      //
+      // Alignment is satisfied, so write them together
+      //
+      Status = BlockIo->WriteBlocks (
+                          BlockIo,
+                          MediaId,
+                          Lba,
+                          WorkingBufferSize,
+                          WorkingBuffer
+                          );
+
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      WorkingBuffer += WorkingBufferSize;
+
+    } else {
+      //
+      // The buffer parameter is not aligned with the request
+      // So use the allocated instead.
+      // It can fit almost all the cases.
+      //
+      LastWrite = FALSE;
+      do {
+        if (WorkingBufferSize <= DataBufferSize) {
+          //
+          // It is the last calling to writeblocks in this loop
+          //
+          DataBufferSize  = WorkingBufferSize;
+          LastWrite       = TRUE;
+        }
+
+        CopyMem (Data, WorkingBuffer, DataBufferSize);
+        Status = BlockIo->WriteBlocks (
+                            BlockIo,
+                            MediaId,
+                            Lba,
+                            DataBufferSize,
+                            Data
+                            );
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+
+        WorkingBufferSize -= DataBufferSize;
+        WorkingBuffer += DataBufferSize;
+        Lba += DATA_BUFFER_BLOCK_NUM;
+      } while (!LastWrite);
+    }
+  }
+
+  if (!TransactionComplete && OverRun != 0) {
+    //
+    // Last bit is not a complete block, so do a read modify write.
+    //
+    Status = BlockIo->ReadBlocks (
+                        BlockIo,
+                        MediaId,
+                        OverRunLba,
+                        BlockSize,
+                        Data
+                        );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    CopyMem (Data, WorkingBuffer, OverRun);
+
+    Status = BlockIo->WriteBlocks (
+                        BlockIo,
+                        MediaId,
+                        OverRunLba,
+                        BlockSize,
+                        Data
+                        );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+  }
+
+Done:
+  if (PreData != NULL) {
+    gBS->FreePool (PreData);
+  }
+
+  return Status;
+}