]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c
MdeModulePkg: Clean up source files
[mirror_edk2.git] / MdeModulePkg / Bus / Scsi / ScsiDiskDxe / ScsiDisk.c
index a85cc94ae6479dad2b13d74dc756b470d1541864..0d63c85e44beb360e359c44b4574ef94c5976a40 100644 (file)
@@ -1,7 +1,7 @@
 /** @file\r
   SCSI disk driver that layers on every SCSI IO protocol in the system.\r
 \r
-Copyright (c) 2006 - 2015, Intel Corporation. All rights reserved.<BR>\r
+Copyright (c) 2006 - 2018, 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
@@ -79,9 +79,9 @@ FreeAlignedBuffer (
 \r
   The user code starts with this function.\r
 \r
-  @param  ImageHandle    The firmware allocated handle for the EFI image.  \r
+  @param  ImageHandle    The firmware allocated handle for the EFI image.\r
   @param  SystemTable    A pointer to the EFI System Table.\r
-  \r
+\r
   @retval EFI_SUCCESS       The entry point is executed successfully.\r
   @retval other             Some error occurs when executing this entry point.\r
 \r
@@ -230,22 +230,27 @@ ScsiDiskDriverBindingStart (
     return Status;\r
   }\r
 \r
-  ScsiDiskDevice->Signature            = SCSI_DISK_DEV_SIGNATURE;\r
-  ScsiDiskDevice->ScsiIo               = ScsiIo;\r
-  ScsiDiskDevice->BlkIo.Revision       = EFI_BLOCK_IO_PROTOCOL_REVISION3;\r
-  ScsiDiskDevice->BlkIo.Media          = &ScsiDiskDevice->BlkIoMedia;\r
-  ScsiDiskDevice->BlkIo.Media->IoAlign = ScsiIo->IoAlign;\r
-  ScsiDiskDevice->BlkIo.Reset          = ScsiDiskReset;\r
-  ScsiDiskDevice->BlkIo.ReadBlocks     = ScsiDiskReadBlocks;\r
-  ScsiDiskDevice->BlkIo.WriteBlocks    = ScsiDiskWriteBlocks;\r
-  ScsiDiskDevice->BlkIo.FlushBlocks    = ScsiDiskFlushBlocks;\r
-  ScsiDiskDevice->BlkIo2.Media         = &ScsiDiskDevice->BlkIoMedia;\r
-  ScsiDiskDevice->BlkIo2.Reset         = ScsiDiskResetEx;\r
-  ScsiDiskDevice->BlkIo2.ReadBlocksEx  = ScsiDiskReadBlocksEx;\r
-  ScsiDiskDevice->BlkIo2.WriteBlocksEx = ScsiDiskWriteBlocksEx;\r
-  ScsiDiskDevice->BlkIo2.FlushBlocksEx = ScsiDiskFlushBlocksEx;\r
-  ScsiDiskDevice->Handle               = Controller;\r
-  InitializeListHead (&ScsiDiskDevice->BlkIo2Queue);\r
+  ScsiDiskDevice->Signature                         = SCSI_DISK_DEV_SIGNATURE;\r
+  ScsiDiskDevice->ScsiIo                            = ScsiIo;\r
+  ScsiDiskDevice->BlkIo.Revision                    = EFI_BLOCK_IO_PROTOCOL_REVISION3;\r
+  ScsiDiskDevice->BlkIo.Media                       = &ScsiDiskDevice->BlkIoMedia;\r
+  ScsiDiskDevice->BlkIo.Media->IoAlign              = ScsiIo->IoAlign;\r
+  ScsiDiskDevice->BlkIo.Reset                       = ScsiDiskReset;\r
+  ScsiDiskDevice->BlkIo.ReadBlocks                  = ScsiDiskReadBlocks;\r
+  ScsiDiskDevice->BlkIo.WriteBlocks                 = ScsiDiskWriteBlocks;\r
+  ScsiDiskDevice->BlkIo.FlushBlocks                 = ScsiDiskFlushBlocks;\r
+  ScsiDiskDevice->BlkIo2.Media                      = &ScsiDiskDevice->BlkIoMedia;\r
+  ScsiDiskDevice->BlkIo2.Reset                      = ScsiDiskResetEx;\r
+  ScsiDiskDevice->BlkIo2.ReadBlocksEx               = ScsiDiskReadBlocksEx;\r
+  ScsiDiskDevice->BlkIo2.WriteBlocksEx              = ScsiDiskWriteBlocksEx;\r
+  ScsiDiskDevice->BlkIo2.FlushBlocksEx              = ScsiDiskFlushBlocksEx;\r
+  ScsiDiskDevice->EraseBlock.Revision               = EFI_ERASE_BLOCK_PROTOCOL_REVISION;\r
+  ScsiDiskDevice->EraseBlock.EraseLengthGranularity = 1;\r
+  ScsiDiskDevice->EraseBlock.EraseBlocks            = ScsiDiskEraseBlocks;\r
+  ScsiDiskDevice->UnmapInfo.MaxBlkDespCnt           = 1;\r
+  ScsiDiskDevice->BlockLimitsVpdSupported           = FALSE;\r
+  ScsiDiskDevice->Handle                            = Controller;\r
+  InitializeListHead (&ScsiDiskDevice->AsyncTaskQueue);\r
 \r
   ScsiIo->GetDeviceType (ScsiIo, &(ScsiDiskDevice->DeviceType));\r
   switch (ScsiDiskDevice->DeviceType) {\r
@@ -323,6 +328,17 @@ ScsiDiskDriverBindingStart (
                       NULL\r
                       );\r
       if (!EFI_ERROR(Status)) {\r
+        if (DetermineInstallEraseBlock(ScsiDiskDevice, Controller)) {\r
+          Status = gBS->InstallProtocolInterface (\r
+                          &Controller,\r
+                          &gEfiEraseBlockProtocolGuid,\r
+                          EFI_NATIVE_INTERFACE,\r
+                          &ScsiDiskDevice->EraseBlock\r
+                          );\r
+          if (EFI_ERROR(Status)) {\r
+            DEBUG ((EFI_D_ERROR, "ScsiDisk: Failed to install the Erase Block Protocol! Status = %r\n", Status));\r
+          }\r
+        }\r
         ScsiDiskDevice->ControllerNameTable = NULL;\r
         AddUnicodeString2 (\r
           "eng",\r
@@ -340,7 +356,7 @@ ScsiDiskDriverBindingStart (
           );\r
         return EFI_SUCCESS;\r
       }\r
-    } \r
+    }\r
   }\r
 \r
   gBS->FreePool (ScsiDiskDevice->SenseData);\r
@@ -352,7 +368,7 @@ ScsiDiskDriverBindingStart (
          Controller\r
          );\r
   return Status;\r
-  \r
+\r
 }\r
 \r
 \r
@@ -364,7 +380,7 @@ ScsiDiskDriverBindingStart (
   restrictions for this service. DisconnectController() must follow these\r
   calling restrictions. If any other agent wishes to call Stop() it must\r
   also follow these calling restrictions.\r
-  \r
+\r
   @param  This              Protocol instance pointer.\r
   @param  ControllerHandle  Handle of device to stop driver on\r
   @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of\r
@@ -384,9 +400,10 @@ ScsiDiskDriverBindingStop (
   IN  EFI_HANDLE                      *ChildHandleBuffer   OPTIONAL\r
   )\r
 {\r
-  EFI_BLOCK_IO_PROTOCOL *BlkIo;\r
-  SCSI_DISK_DEV         *ScsiDiskDevice;\r
-  EFI_STATUS            Status;\r
+  EFI_BLOCK_IO_PROTOCOL      *BlkIo;\r
+  EFI_ERASE_BLOCK_PROTOCOL   *EraseBlock;\r
+  SCSI_DISK_DEV              *ScsiDiskDevice;\r
+  EFI_STATUS                 Status;\r
 \r
   Status = gBS->OpenProtocol (\r
                   Controller,\r
@@ -405,7 +422,30 @@ ScsiDiskDriverBindingStop (
   //\r
   // Wait for the BlockIo2 requests queue to become empty\r
   //\r
-  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));\r
+  while (!IsListEmpty (&ScsiDiskDevice->AsyncTaskQueue));\r
+\r
+  //\r
+  // If Erase Block Protocol is installed, then uninstall this protocol.\r
+  //\r
+  Status = gBS->OpenProtocol (\r
+                  Controller,\r
+                  &gEfiEraseBlockProtocolGuid,\r
+                  (VOID **) &EraseBlock,\r
+                  This->DriverBindingHandle,\r
+                  Controller,\r
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL\r
+                  );\r
+\r
+  if (!EFI_ERROR (Status)) {\r
+    Status = gBS->UninstallProtocolInterface (\r
+                    Controller,\r
+                    &gEfiEraseBlockProtocolGuid,\r
+                    &ScsiDiskDevice->EraseBlock\r
+                    );\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
+  }\r
 \r
   Status = gBS->UninstallMultipleProtocolInterfaces (\r
                   Controller,\r
@@ -466,8 +506,12 @@ ScsiDiskReset (
   Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);\r
 \r
   if (EFI_ERROR (Status)) {\r
-    Status = EFI_DEVICE_ERROR;\r
-    goto Done;\r
+    if (Status == EFI_UNSUPPORTED) {\r
+      Status = EFI_SUCCESS;\r
+    } else {\r
+      Status = EFI_DEVICE_ERROR;\r
+      goto Done;\r
+    }\r
   }\r
 \r
   if (!ExtendedVerification) {\r
@@ -524,6 +568,7 @@ ScsiDiskReadBlocks (
   MediaChange    = FALSE;\r
   OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
   ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);\r
+  Media          = ScsiDiskDevice->BlkIo.Media;\r
 \r
   if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
 \r
@@ -546,14 +591,25 @@ ScsiDiskReadBlocks (
              &ScsiDiskDevice->BlkIo2,\r
              &ScsiDiskDevice->BlkIo2\r
              );\r
-      Status = EFI_MEDIA_CHANGED;\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      if (Media->MediaPresent) {\r
+        Status = EFI_MEDIA_CHANGED;\r
+      } else {\r
+        Status = EFI_NO_MEDIA;\r
+      }\r
       goto Done;\r
     }\r
   }\r
   //\r
   // Get the intrinsic block size\r
   //\r
-  Media           = ScsiDiskDevice->BlkIo.Media;\r
   BlockSize       = Media->BlockSize;\r
 \r
   NumberOfBlocks  = BufferSize / BlockSize;\r
@@ -648,6 +704,7 @@ ScsiDiskWriteBlocks (
   MediaChange    = FALSE;\r
   OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
   ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);\r
+  Media          = ScsiDiskDevice->BlkIo.Media;\r
 \r
   if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
 \r
@@ -670,14 +727,25 @@ ScsiDiskWriteBlocks (
              &ScsiDiskDevice->BlkIo2,\r
              &ScsiDiskDevice->BlkIo2\r
              );\r
-      Status = EFI_MEDIA_CHANGED;\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      if (Media->MediaPresent) {\r
+        Status = EFI_MEDIA_CHANGED;\r
+      } else {\r
+        Status = EFI_NO_MEDIA;\r
+      }\r
       goto Done;\r
     }\r
   }\r
   //\r
   // Get the intrinsic block size\r
   //\r
-  Media           = ScsiDiskDevice->BlkIo.Media;\r
   BlockSize       = Media->BlockSize;\r
 \r
   NumberOfBlocks  = BufferSize / BlockSize;\r
@@ -692,6 +760,11 @@ ScsiDiskWriteBlocks (
     goto Done;\r
   }\r
 \r
+  if (Media->ReadOnly) {\r
+    Status = EFI_WRITE_PROTECTED;\r
+    goto Done;\r
+  }\r
+\r
   if (BufferSize == 0) {\r
     Status = EFI_SUCCESS;\r
     goto Done;\r
@@ -785,8 +858,12 @@ ScsiDiskResetEx (
   Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);\r
 \r
   if (EFI_ERROR (Status)) {\r
-    Status = EFI_DEVICE_ERROR;\r
-    goto Done;\r
+    if (Status == EFI_UNSUPPORTED) {\r
+      Status = EFI_SUCCESS;\r
+    } else {\r
+      Status = EFI_DEVICE_ERROR;\r
+      goto Done;\r
+    }\r
   }\r
 \r
   if (!ExtendedVerification) {\r
@@ -853,6 +930,7 @@ ScsiDiskReadBlocksEx (
   MediaChange    = FALSE;\r
   OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
   ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);\r
+  Media          = ScsiDiskDevice->BlkIo.Media;\r
 \r
   if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
 \r
@@ -875,14 +953,25 @@ ScsiDiskReadBlocksEx (
              &ScsiDiskDevice->BlkIo2,\r
              &ScsiDiskDevice->BlkIo2\r
              );\r
-      Status = EFI_MEDIA_CHANGED;\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      if (Media->MediaPresent) {\r
+        Status = EFI_MEDIA_CHANGED;\r
+      } else {\r
+        Status = EFI_NO_MEDIA;\r
+      }\r
       goto Done;\r
     }\r
   }\r
   //\r
   // Get the intrinsic block size\r
   //\r
-  Media           = ScsiDiskDevice->BlkIo2.Media;\r
   BlockSize       = Media->BlockSize;\r
 \r
   NumberOfBlocks  = BufferSize / BlockSize;\r
@@ -1004,6 +1093,7 @@ ScsiDiskWriteBlocksEx (
   MediaChange    = FALSE;\r
   OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
   ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);\r
+  Media          = ScsiDiskDevice->BlkIo.Media;\r
 \r
   if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
 \r
@@ -1026,14 +1116,25 @@ ScsiDiskWriteBlocksEx (
              &ScsiDiskDevice->BlkIo2,\r
              &ScsiDiskDevice->BlkIo2\r
              );\r
-      Status = EFI_MEDIA_CHANGED;\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      if (Media->MediaPresent) {\r
+        Status = EFI_MEDIA_CHANGED;\r
+      } else {\r
+        Status = EFI_NO_MEDIA;\r
+      }\r
       goto Done;\r
     }\r
   }\r
   //\r
   // Get the intrinsic block size\r
   //\r
-  Media           = ScsiDiskDevice->BlkIo2.Media;\r
   BlockSize       = Media->BlockSize;\r
 \r
   NumberOfBlocks  = BufferSize / BlockSize;\r
@@ -1048,6 +1149,11 @@ ScsiDiskWriteBlocksEx (
     goto Done;\r
   }\r
 \r
+  if (Media->ReadOnly) {\r
+    Status = EFI_WRITE_PROTECTED;\r
+    goto Done;\r
+  }\r
+\r
   if (BufferSize == 0) {\r
     if ((Token != NULL) && (Token->Event != NULL)) {\r
       Token->TransactionStatus = EFI_SUCCESS;\r
@@ -1140,6 +1246,7 @@ ScsiDiskFlushBlocksEx (
   MediaChange    = FALSE;\r
   OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
   ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);\r
+  Media          = ScsiDiskDevice->BlkIo.Media;\r
 \r
   if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
 \r
@@ -1162,13 +1269,23 @@ ScsiDiskFlushBlocksEx (
              &ScsiDiskDevice->BlkIo2,\r
              &ScsiDiskDevice->BlkIo2\r
              );\r
-      Status = EFI_MEDIA_CHANGED;\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      if (Media->MediaPresent) {\r
+        Status = EFI_MEDIA_CHANGED;\r
+      } else {\r
+        Status = EFI_NO_MEDIA;\r
+      }\r
       goto Done;\r
     }\r
   }\r
 \r
-  Media = ScsiDiskDevice->BlkIo2.Media;\r
-\r
   if (!(Media->MediaPresent)) {\r
     Status = EFI_NO_MEDIA;\r
     goto Done;\r
@@ -1182,7 +1299,7 @@ ScsiDiskFlushBlocksEx (
   //\r
   // Wait for the BlockIo2 requests queue to become empty\r
   //\r
-  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));\r
+  while (!IsListEmpty (&ScsiDiskDevice->AsyncTaskQueue));\r
 \r
   Status = EFI_SUCCESS;\r
 \r
@@ -1200,12 +1317,410 @@ Done:
 }\r
 \r
 \r
+/**\r
+  Internal helper notify function which process the result of an asynchronous\r
+  SCSI UNMAP Command and signal the event passed from EraseBlocks.\r
+\r
+  @param  Event    The instance of EFI_EVENT.\r
+  @param  Context  The parameter passed in.\r
+\r
+**/\r
+VOID\r
+EFIAPI\r
+ScsiDiskAsyncUnmapNotify (\r
+  IN  EFI_EVENT  Event,\r
+  IN  VOID       *Context\r
+  )\r
+{\r
+  SCSI_ERASEBLK_REQUEST            *EraseBlkReq;\r
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET  *CommandPacket;\r
+  EFI_ERASE_BLOCK_TOKEN            *Token;\r
+  EFI_STATUS                       Status;\r
+\r
+  gBS->CloseEvent (Event);\r
+\r
+  EraseBlkReq              = (SCSI_ERASEBLK_REQUEST *) Context;\r
+  CommandPacket            = &EraseBlkReq->CommandPacket;\r
+  Token                    = EraseBlkReq->Token;\r
+  Token->TransactionStatus = EFI_SUCCESS;\r
+\r
+  Status = CheckHostAdapterStatus (CommandPacket->HostAdapterStatus);\r
+  if (EFI_ERROR(Status)) {\r
+    DEBUG ((\r
+      EFI_D_ERROR,\r
+      "ScsiDiskAsyncUnmapNotify: Host adapter indicating error status 0x%x.\n",\r
+      CommandPacket->HostAdapterStatus\r
+      ));\r
+\r
+    Token->TransactionStatus = Status;\r
+    goto Done;\r
+  }\r
+\r
+  Status = CheckTargetStatus (CommandPacket->TargetStatus);\r
+  if (EFI_ERROR(Status)) {\r
+    DEBUG ((\r
+      EFI_D_ERROR,\r
+      "ScsiDiskAsyncUnmapNotify: Target indicating error status 0x%x.\n",\r
+      CommandPacket->HostAdapterStatus\r
+      ));\r
+\r
+    Token->TransactionStatus = Status;\r
+    goto Done;\r
+  }\r
+\r
+Done:\r
+  RemoveEntryList (&EraseBlkReq->Link);\r
+  FreePool (CommandPacket->OutDataBuffer);\r
+  FreePool (EraseBlkReq->CommandPacket.Cdb);\r
+  FreePool (EraseBlkReq);\r
+\r
+  gBS->SignalEvent (Token->Event);\r
+}\r
+\r
+/**\r
+  Require the device server to cause one or more LBAs to be unmapped.\r
+\r
+  @param  ScsiDiskDevice         The pointer of ScsiDiskDevice.\r
+  @param  Lba                    The start block number.\r
+  @param  Blocks                 Total block number to be unmapped.\r
+  @param  Token                  The pointer to the token associated with the\r
+                                 non-blocking erase block request.\r
+\r
+  @retval EFI_SUCCESS            Target blocks have been successfully unmapped.\r
+  @retval EFI_DEVICE_ERROR       Fail to unmap the target blocks.\r
+\r
+**/\r
+EFI_STATUS\r
+ScsiDiskUnmap (\r
+  IN SCSI_DISK_DEV                 *ScsiDiskDevice,\r
+  IN UINT64                        Lba,\r
+  IN UINTN                         Blocks,\r
+  IN EFI_ERASE_BLOCK_TOKEN         *Token            OPTIONAL\r
+  )\r
+{\r
+  EFI_SCSI_IO_PROTOCOL             *ScsiIo;\r
+  SCSI_ERASEBLK_REQUEST            *EraseBlkReq;\r
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET  *CommandPacket;\r
+  EFI_SCSI_DISK_UNMAP_BLOCK_DESP   *BlkDespPtr;\r
+  EFI_STATUS                       Status;\r
+  EFI_STATUS                       ReturnStatus;\r
+  UINT8                            *Cdb;\r
+  UINT32                           MaxLbaCnt;\r
+  UINT32                           MaxBlkDespCnt;\r
+  UINT32                           BlkDespCnt;\r
+  UINT16                           UnmapParamListLen;\r
+  VOID                             *UnmapParamList;\r
+  EFI_EVENT                        AsyncUnmapEvent;\r
+  EFI_TPL                          OldTpl;\r
+\r
+  ScsiIo          = ScsiDiskDevice->ScsiIo;\r
+  MaxLbaCnt       = ScsiDiskDevice->UnmapInfo.MaxLbaCnt;\r
+  MaxBlkDespCnt   = ScsiDiskDevice->UnmapInfo.MaxBlkDespCnt;\r
+  EraseBlkReq     = NULL;\r
+  UnmapParamList  = NULL;\r
+  AsyncUnmapEvent = NULL;\r
+  ReturnStatus    = EFI_SUCCESS;\r
+\r
+  if (Blocks / (UINTN) MaxLbaCnt > MaxBlkDespCnt) {\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+  EraseBlkReq = AllocateZeroPool (sizeof (SCSI_ERASEBLK_REQUEST));\r
+  if (EraseBlkReq == NULL) {\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+  EraseBlkReq->CommandPacket.Cdb = AllocateZeroPool (0xA);\r
+  if (EraseBlkReq->CommandPacket.Cdb == NULL) {\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+  BlkDespCnt        = (UINT32) ((Blocks - 1) / MaxLbaCnt + 1);\r
+  UnmapParamListLen = (UINT16) (sizeof (EFI_SCSI_DISK_UNMAP_PARAM_LIST_HEADER)\r
+                      + BlkDespCnt * sizeof (EFI_SCSI_DISK_UNMAP_BLOCK_DESP));\r
+  UnmapParamList    = AllocateZeroPool (UnmapParamListLen);\r
+  if (UnmapParamList == NULL) {\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+  *((UINT16 *)UnmapParamList)     = SwapBytes16 (UnmapParamListLen - 2);\r
+  *((UINT16 *)UnmapParamList + 1) = SwapBytes16 (UnmapParamListLen - sizeof (EFI_SCSI_DISK_UNMAP_PARAM_LIST_HEADER));\r
+\r
+  BlkDespPtr = (EFI_SCSI_DISK_UNMAP_BLOCK_DESP *)((UINT8 *)UnmapParamList + sizeof (EFI_SCSI_DISK_UNMAP_PARAM_LIST_HEADER));\r
+  while (Blocks > 0) {\r
+    if (Blocks > MaxLbaCnt) {\r
+      *(UINT64 *)(&BlkDespPtr->Lba)      = SwapBytes64 (Lba);\r
+      *(UINT32 *)(&BlkDespPtr->BlockNum) = SwapBytes32 (MaxLbaCnt);\r
+      Blocks -= MaxLbaCnt;\r
+      Lba    += MaxLbaCnt;\r
+    } else {\r
+      *(UINT64 *)(&BlkDespPtr->Lba)      = SwapBytes64 (Lba);\r
+      *(UINT32 *)(&BlkDespPtr->BlockNum) = SwapBytes32 ((UINT32) Blocks);\r
+      Blocks = 0;\r
+    }\r
+\r
+    BlkDespPtr++;\r
+  }\r
+\r
+  CommandPacket                    = &EraseBlkReq->CommandPacket;\r
+  CommandPacket->Timeout           = SCSI_DISK_TIMEOUT;\r
+  CommandPacket->OutDataBuffer     = UnmapParamList;\r
+  CommandPacket->OutTransferLength = UnmapParamListLen;\r
+  CommandPacket->CdbLength         = 0xA;\r
+  CommandPacket->DataDirection     = EFI_SCSI_DATA_OUT;\r
+  //\r
+  // Fill Cdb for UNMAP Command\r
+  //\r
+  Cdb    = CommandPacket->Cdb;\r
+  Cdb[0] = EFI_SCSI_OP_UNMAP;\r
+  WriteUnaligned16 ((UINT16 *)&Cdb[7], SwapBytes16 (UnmapParamListLen));\r
+\r
+  if ((Token != NULL) && (Token->Event != NULL)) {\r
+    //\r
+    // Non-blocking UNMAP request\r
+    //\r
+    Status = gBS->CreateEvent (\r
+                    EVT_NOTIFY_SIGNAL,\r
+                    TPL_NOTIFY,\r
+                    ScsiDiskAsyncUnmapNotify,\r
+                    EraseBlkReq,\r
+                    &AsyncUnmapEvent\r
+                    );\r
+    if (EFI_ERROR(Status)) {\r
+      ReturnStatus = EFI_DEVICE_ERROR;\r
+      goto Done;\r
+    }\r
+\r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+    InsertTailList (&ScsiDiskDevice->AsyncTaskQueue, &EraseBlkReq->Link);\r
+    gBS->RestoreTPL (OldTpl);\r
+\r
+    EraseBlkReq->Token = Token;\r
+\r
+    Status = ScsiIo->ExecuteScsiCommand (\r
+                       ScsiIo,\r
+                       CommandPacket,\r
+                       AsyncUnmapEvent\r
+                       );\r
+    if (EFI_ERROR(Status)) {\r
+      ReturnStatus = EFI_DEVICE_ERROR;\r
+\r
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+      RemoveEntryList (&EraseBlkReq->Link);\r
+      gBS->RestoreTPL (OldTpl);\r
+\r
+      goto Done;\r
+    } else {\r
+      //\r
+      // Directly return if the non-blocking UNMAP request is queued.\r
+      //\r
+      return EFI_SUCCESS;\r
+    }\r
+  } else {\r
+    //\r
+    // Blocking UNMAP request\r
+    //\r
+    Status = ScsiIo->ExecuteScsiCommand (\r
+                       ScsiIo,\r
+                       CommandPacket,\r
+                       NULL\r
+                       );\r
+    if (EFI_ERROR(Status)) {\r
+      ReturnStatus = EFI_DEVICE_ERROR;\r
+      goto Done;\r
+    }\r
+  }\r
+\r
+  //\r
+  // Only blocking UNMAP request will reach here.\r
+  //\r
+  Status = CheckHostAdapterStatus (CommandPacket->HostAdapterStatus);\r
+  if (EFI_ERROR(Status)) {\r
+    DEBUG ((\r
+      EFI_D_ERROR,\r
+      "ScsiDiskUnmap: Host adapter indicating error status 0x%x.\n",\r
+      CommandPacket->HostAdapterStatus\r
+      ));\r
+\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+  Status = CheckTargetStatus (CommandPacket->TargetStatus);\r
+  if (EFI_ERROR(Status)) {\r
+    DEBUG ((\r
+      EFI_D_ERROR,\r
+      "ScsiDiskUnmap: Target indicating error status 0x%x.\n",\r
+      CommandPacket->HostAdapterStatus\r
+      ));\r
+\r
+    ReturnStatus = EFI_DEVICE_ERROR;\r
+    goto Done;\r
+  }\r
+\r
+Done:\r
+  if (EraseBlkReq != NULL) {\r
+    if (EraseBlkReq->CommandPacket.Cdb != NULL) {\r
+      FreePool (EraseBlkReq->CommandPacket.Cdb);\r
+    }\r
+    FreePool (EraseBlkReq);\r
+  }\r
+\r
+  if (UnmapParamList != NULL) {\r
+    FreePool (UnmapParamList);\r
+  }\r
+\r
+  if (AsyncUnmapEvent != NULL) {\r
+    gBS->CloseEvent (AsyncUnmapEvent);\r
+  }\r
+\r
+  return ReturnStatus;\r
+}\r
+\r
+/**\r
+  Erase a specified number of device blocks.\r
+\r
+  @param[in]       This           Indicates a pointer to the calling context.\r
+  @param[in]       MediaId        The media ID that the erase request is for.\r
+  @param[in]       Lba            The starting logical block address to be\r
+                                  erased. The caller is responsible for erasing\r
+                                  only legitimate locations.\r
+  @param[in, out]  Token          A pointer to the token associated with the\r
+                                  transaction.\r
+  @param[in]       Size           The size in bytes to be erased. This must be\r
+                                  a multiple of the physical block size of the\r
+                                  device.\r
+\r
+  @retval EFI_SUCCESS             The erase request was queued if Event is not\r
+                                  NULL. The data was erased correctly to the\r
+                                  device if the Event is NULL.to the device.\r
+  @retval EFI_WRITE_PROTECTED     The device cannot be erased due to write\r
+                                  protection.\r
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting\r
+                                  to perform the erase operation.\r
+  @retval EFI_INVALID_PARAMETER   The erase request contains LBAs that are not\r
+                                  valid.\r
+  @retval EFI_NO_MEDIA            There is no media in the device.\r
+  @retval EFI_MEDIA_CHANGED       The MediaId is not for the current media.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+ScsiDiskEraseBlocks (\r
+  IN     EFI_ERASE_BLOCK_PROTOCOL      *This,\r
+  IN     UINT32                        MediaId,\r
+  IN     EFI_LBA                       Lba,\r
+  IN OUT EFI_ERASE_BLOCK_TOKEN         *Token,\r
+  IN     UINTN                         Size\r
+  )\r
+{\r
+  SCSI_DISK_DEV       *ScsiDiskDevice;\r
+  EFI_BLOCK_IO_MEDIA  *Media;\r
+  EFI_STATUS          Status;\r
+  UINTN               BlockSize;\r
+  UINTN               NumberOfBlocks;\r
+  BOOLEAN             MediaChange;\r
+  EFI_TPL             OldTpl;\r
+\r
+  MediaChange    = FALSE;\r
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);\r
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_ERASEBLK (This);\r
+\r
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {\r
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);\r
+    if (EFI_ERROR (Status)) {\r
+      Status = EFI_DEVICE_ERROR;\r
+      goto Done;\r
+    }\r
+\r
+    if (MediaChange) {\r
+      gBS->ReinstallProtocolInterface (\r
+            ScsiDiskDevice->Handle,\r
+            &gEfiBlockIoProtocolGuid,\r
+            &ScsiDiskDevice->BlkIo,\r
+            &ScsiDiskDevice->BlkIo\r
+            );\r
+      gBS->ReinstallProtocolInterface (\r
+             ScsiDiskDevice->Handle,\r
+             &gEfiBlockIo2ProtocolGuid,\r
+             &ScsiDiskDevice->BlkIo2,\r
+             &ScsiDiskDevice->BlkIo2\r
+             );\r
+      if (DetermineInstallEraseBlock(ScsiDiskDevice, ScsiDiskDevice->Handle)) {\r
+        gBS->ReinstallProtocolInterface (\r
+               ScsiDiskDevice->Handle,\r
+               &gEfiEraseBlockProtocolGuid,\r
+               &ScsiDiskDevice->EraseBlock,\r
+               &ScsiDiskDevice->EraseBlock\r
+               );\r
+      }\r
+      Status = EFI_MEDIA_CHANGED;\r
+      goto Done;\r
+    }\r
+  }\r
+  //\r
+  // Get the intrinsic block size\r
+  //\r
+  Media = ScsiDiskDevice->BlkIo.Media;\r
+\r
+  if (!(Media->MediaPresent)) {\r
+    Status = EFI_NO_MEDIA;\r
+    goto Done;\r
+  }\r
+\r
+  if (MediaId != Media->MediaId) {\r
+    Status = EFI_MEDIA_CHANGED;\r
+    goto Done;\r
+  }\r
+\r
+  if (Media->ReadOnly) {\r
+    Status = EFI_WRITE_PROTECTED;\r
+    goto Done;\r
+  }\r
+\r
+  if (Size == 0) {\r
+    if ((Token != NULL) && (Token->Event != NULL)) {\r
+      Token->TransactionStatus = EFI_SUCCESS;\r
+      gBS->SignalEvent (Token->Event);\r
+    }\r
+    Status = EFI_SUCCESS;\r
+    goto Done;\r
+  }\r
+\r
+  BlockSize = Media->BlockSize;\r
+  if ((Size % BlockSize) != 0) {\r
+    Status = EFI_INVALID_PARAMETER;\r
+    goto Done;\r
+  }\r
+\r
+  NumberOfBlocks = Size / BlockSize;\r
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {\r
+    Status = EFI_INVALID_PARAMETER;\r
+    goto Done;\r
+  }\r
+\r
+  if ((Token != NULL) && (Token->Event != NULL)) {\r
+    Status = ScsiDiskUnmap (ScsiDiskDevice, Lba, NumberOfBlocks, Token);\r
+  } else {\r
+    Status = ScsiDiskUnmap (ScsiDiskDevice, Lba, NumberOfBlocks, NULL);\r
+  }\r
+\r
+Done:\r
+  gBS->RestoreTPL (OldTpl);\r
+  return Status;\r
+}\r
+\r
+\r
 /**\r
   Detect Device and read out capacity ,if error occurs, parse the sense key.\r
 \r
   @param  ScsiDiskDevice    The pointer of SCSI_DISK_DEV\r
   @param  MustReadCapacity  The flag about reading device capacity\r
-  @param  MediaChange       The pointer of flag indicates if media has changed \r
+  @param  MediaChange       The pointer of flag indicates if media has changed\r
 \r
   @retval EFI_DEVICE_ERROR  Indicates that error occurs\r
   @retval EFI_SUCCESS       Successfully to detect media\r
@@ -1340,7 +1855,7 @@ ScsiDiskDetectMedia (
         } else {\r
           break;\r
         }\r
-      } else {   \r
+      } else {\r
         Retry++;\r
         if (!NeedRetry || (Retry >= MaxRetry)) {\r
           goto EXIT;\r
@@ -1475,7 +1990,44 @@ ScsiDiskInquiryDevice (
       if (!EFI_ERROR (Status)) {\r
         PageLength = (SupportedVpdPages->PageLength2 << 8)\r
                    |  SupportedVpdPages->PageLength1;\r
+\r
+        //\r
+        // Sanity checks for coping with broken devices\r
+        //\r
+        if (PageLength > sizeof SupportedVpdPages->SupportedVpdPageList) {\r
+          DEBUG ((EFI_D_WARN,\r
+            "%a: invalid PageLength (%u) in Supported VPD Pages page\n",\r
+            __FUNCTION__, (UINT32)PageLength));\r
+          PageLength = 0;\r
+        }\r
+\r
+        if ((PageLength > 0) &&\r
+            (SupportedVpdPages->SupportedVpdPageList[0] !=\r
+             EFI_SCSI_PAGE_CODE_SUPPORTED_VPD)) {\r
+          DEBUG ((EFI_D_WARN,\r
+            "%a: Supported VPD Pages page doesn't start with code 0x%02x\n",\r
+            __FUNCTION__, EFI_SCSI_PAGE_CODE_SUPPORTED_VPD));\r
+          PageLength = 0;\r
+        }\r
+\r
+        //\r
+        // Locate the code for the Block Limits VPD page\r
+        //\r
         for (Index = 0; Index < PageLength; Index++) {\r
+          //\r
+          // Sanity check\r
+          //\r
+          if ((Index > 0) &&\r
+              (SupportedVpdPages->SupportedVpdPageList[Index] <=\r
+               SupportedVpdPages->SupportedVpdPageList[Index - 1])) {\r
+            DEBUG ((EFI_D_WARN,\r
+              "%a: non-ascending code in Supported VPD Pages page @ %u\n",\r
+              __FUNCTION__, Index));\r
+            Index = 0;\r
+            PageLength = 0;\r
+            break;\r
+          }\r
+\r
           if (SupportedVpdPages->SupportedVpdPageList[Index] == EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD) {\r
             break;\r
           }\r
@@ -1507,9 +2059,42 @@ ScsiDiskInquiryDevice (
                      EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD\r
                      );\r
           if (!EFI_ERROR (Status)) {\r
-            ScsiDiskDevice->BlkIo.Media->OptimalTransferLengthGranularity = \r
+            ScsiDiskDevice->BlkIo.Media->OptimalTransferLengthGranularity =\r
               (BlockLimits->OptimalTransferLengthGranularity2 << 8) |\r
                BlockLimits->OptimalTransferLengthGranularity1;\r
+\r
+            ScsiDiskDevice->UnmapInfo.MaxLbaCnt =\r
+              (BlockLimits->MaximumUnmapLbaCount4 << 24) |\r
+              (BlockLimits->MaximumUnmapLbaCount3 << 16) |\r
+              (BlockLimits->MaximumUnmapLbaCount2 << 8)  |\r
+              BlockLimits->MaximumUnmapLbaCount1;\r
+            ScsiDiskDevice->UnmapInfo.MaxBlkDespCnt =\r
+              (BlockLimits->MaximumUnmapBlockDescriptorCount4 << 24) |\r
+              (BlockLimits->MaximumUnmapBlockDescriptorCount3 << 16) |\r
+              (BlockLimits->MaximumUnmapBlockDescriptorCount2 << 8)  |\r
+              BlockLimits->MaximumUnmapBlockDescriptorCount1;\r
+            ScsiDiskDevice->EraseBlock.EraseLengthGranularity =\r
+              (BlockLimits->OptimalUnmapGranularity4 << 24) |\r
+              (BlockLimits->OptimalUnmapGranularity3 << 16) |\r
+              (BlockLimits->OptimalUnmapGranularity2 << 8)  |\r
+              BlockLimits->OptimalUnmapGranularity1;\r
+            if (BlockLimits->UnmapGranularityAlignmentValid != 0) {\r
+              ScsiDiskDevice->UnmapInfo.GranularityAlignment =\r
+                (BlockLimits->UnmapGranularityAlignment4 << 24) |\r
+                (BlockLimits->UnmapGranularityAlignment3 << 16) |\r
+                (BlockLimits->UnmapGranularityAlignment2 << 8)  |\r
+                BlockLimits->UnmapGranularityAlignment1;\r
+            }\r
+\r
+            if (ScsiDiskDevice->EraseBlock.EraseLengthGranularity == 0) {\r
+              //\r
+              // A value of 0 indicates that the optimal unmap granularity is\r
+              // not reported.\r
+              //\r
+              ScsiDiskDevice->EraseBlock.EraseLengthGranularity = 1;\r
+            }\r
+\r
+            ScsiDiskDevice->BlockLimitsVpdSupported = TRUE;\r
           }\r
 \r
           FreeAlignedBuffer (BlockLimits, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));\r
@@ -1526,7 +2111,7 @@ ScsiDiskInquiryDevice (
   } else if (Status == EFI_NOT_READY) {\r
     *NeedRetry = TRUE;\r
     return EFI_DEVICE_ERROR;\r
\r
+\r
   } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {\r
     *NeedRetry = FALSE;\r
     return EFI_DEVICE_ERROR;\r
@@ -1562,7 +2147,7 @@ ScsiDiskInquiryDevice (
     *NeedRetry = FALSE;\r
     return EFI_DEVICE_ERROR;\r
   }\r
-  \r
+\r
   //\r
   // if goes here, meant ScsiInquiryCommand() failed.\r
   // if ScsiDiskRequestSenseKeys() succeeds at last,\r
@@ -1718,7 +2303,7 @@ ScsiDiskTestUnitReady (
 \r
   @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV\r
   @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA\r
-  @param  NumberOfSenseKeys  The number of sense key  \r
+  @param  NumberOfSenseKeys  The number of sense key\r
   @param  Action             The pointer of action which indicates what is need to do next\r
 \r
   @retval EFI_DEVICE_ERROR   Indicates that error occurs\r
@@ -1859,7 +2444,7 @@ ScsiDiskReadCapacity (
   *NeedRetry          = FALSE;\r
 \r
   //\r
-  // submit Read Capacity(10) Command. If it returns capacity of FFFFFFFFh, \r
+  // submit Read Capacity(10) Command. If it returns capacity of FFFFFFFFh,\r
   // 16 byte command should be used to access large hard disk >2TB\r
   //\r
   CommandStatus = ScsiReadCapacityCommand (\r
@@ -1923,12 +2508,12 @@ ScsiDiskReadCapacity (
    // go ahead to check HostAdapterStatus and TargetStatus\r
    // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)\r
    //\r
\r
+\r
    Status = CheckHostAdapterStatus (HostAdapterStatus);\r
    if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {\r
      *NeedRetry = TRUE;\r
      return EFI_DEVICE_ERROR;\r
\r
+\r
    } else if (Status == EFI_DEVICE_ERROR) {\r
     //\r
     // reset the scsi channel\r
@@ -1951,7 +2536,7 @@ ScsiDiskReadCapacity (
     *NeedRetry = FALSE;\r
     return EFI_DEVICE_ERROR;\r
   }\r
-  \r
+\r
   //\r
   // if goes here, meant ScsiReadCapacityCommand() failed.\r
   // if ScsiDiskRequestSenseKeys() succeeds at last,\r
@@ -2031,7 +2616,7 @@ CheckHostAdapterStatus (
   @param  TargetStatus  Target status\r
 \r
   @retval EFI_NOT_READY       Device is NOT ready.\r
-  @retval EFI_DEVICE_ERROR \r
+  @retval EFI_DEVICE_ERROR\r
   @retval EFI_SUCCESS\r
 \r
 **/\r
@@ -2123,20 +2708,20 @@ ScsiDiskRequestSenseKeys (
               );\r
      if ((Status == EFI_SUCCESS) || (Status == EFI_WARN_BUFFER_TOO_SMALL)) {\r
         FallStatus = EFI_SUCCESS;\r
-  \r
+\r
      } else if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {\r
        *NeedRetry  = TRUE;\r
        FallStatus  = EFI_DEVICE_ERROR;\r
\r
+\r
      } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {\r
        *NeedRetry  = FALSE;\r
        FallStatus  = EFI_DEVICE_ERROR;\r
\r
+\r
      } else if (Status == EFI_DEVICE_ERROR) {\r
         if (AskResetIfError) {\r
           ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);\r
         }\r
-  \r
+\r
         FallStatus = EFI_DEVICE_ERROR;\r
     }\r
 \r
@@ -2158,7 +2743,7 @@ ScsiDiskRequestSenseKeys (
     // no more sense key or number of sense keys exceeds predefined,\r
     // skip the loop.\r
     //\r
-    if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) || \r
+    if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) ||\r
         (*NumberOfSenseKeys == ScsiDiskDevice->SenseDataNumber)) {\r
       SenseReq = FALSE;\r
     }\r
@@ -2188,17 +2773,20 @@ GetMediaInfo (
   UINT8       *Ptr;\r
 \r
   if (!ScsiDiskDevice->Cdb16Byte) {\r
-    ScsiDiskDevice->BlkIo.Media->LastBlock =  (Capacity10->LastLba3 << 24) |\r
+    ScsiDiskDevice->BlkIo.Media->LastBlock =  ((UINT32) Capacity10->LastLba3 << 24) |\r
                                               (Capacity10->LastLba2 << 16) |\r
                                               (Capacity10->LastLba1 << 8)  |\r
                                                Capacity10->LastLba0;\r
-  \r
+\r
     ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity10->BlockSize3 << 24) |\r
-                                             (Capacity10->BlockSize2 << 16) | \r
+                                             (Capacity10->BlockSize2 << 16) |\r
                                              (Capacity10->BlockSize1 << 8)  |\r
                                               Capacity10->BlockSize0;\r
     ScsiDiskDevice->BlkIo.Media->LowestAlignedLba               = 0;\r
     ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = 0;\r
+    if (!ScsiDiskDevice->BlockLimitsVpdSupported) {\r
+      ScsiDiskDevice->UnmapInfo.MaxLbaCnt = (UINT32) ScsiDiskDevice->BlkIo.Media->LastBlock;\r
+    }\r
   } else {\r
     Ptr = (UINT8*)&ScsiDiskDevice->BlkIo.Media->LastBlock;\r
     *Ptr++ = Capacity16->LastLba0;\r
@@ -2211,13 +2799,20 @@ GetMediaInfo (
     *Ptr   = Capacity16->LastLba7;\r
 \r
     ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity16->BlockSize3 << 24) |\r
-                                             (Capacity16->BlockSize2 << 16) | \r
+                                             (Capacity16->BlockSize2 << 16) |\r
                                              (Capacity16->BlockSize1 << 8)  |\r
                                               Capacity16->BlockSize0;\r
 \r
     ScsiDiskDevice->BlkIo.Media->LowestAlignedLba = (Capacity16->LowestAlignLogic2 << 8) |\r
                                                      Capacity16->LowestAlignLogic1;\r
     ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = (1 << Capacity16->LogicPerPhysical);\r
+    if (!ScsiDiskDevice->BlockLimitsVpdSupported) {\r
+      if (ScsiDiskDevice->BlkIo.Media->LastBlock > (UINT32) -1) {\r
+        ScsiDiskDevice->UnmapInfo.MaxLbaCnt = (UINT32) -1;\r
+      } else {\r
+        ScsiDiskDevice->UnmapInfo.MaxLbaCnt = (UINT32) ScsiDiskDevice->BlkIo.Media->LastBlock;\r
+      }\r
+    }\r
   }\r
 \r
   ScsiDiskDevice->BlkIo.Media->MediaPresent = TRUE;\r
@@ -2275,7 +2870,7 @@ ScsiDiskReadSectors (
 \r
   BlocksRemaining   = NumberOfBlocks;\r
   BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;\r
-  \r
+\r
   //\r
   // limit the data bytes that can be transferred by one Read(10) or Read(16) Command\r
   //\r
@@ -2519,7 +3114,7 @@ ScsiDiskWriteSectors (
                   &ByteCount,\r
                   Lba,\r
                   SectorCount\r
-                  );         \r
+                  );\r
         }\r
       if (!EFI_ERROR (Status)) {\r
         break;\r
@@ -2599,6 +3194,7 @@ ScsiDiskAsyncReadSectors (
   UINT64                Timeout;\r
   SCSI_BLKIO2_REQUEST   *BlkIo2Req;\r
   EFI_STATUS            Status;\r
+  EFI_TPL               OldTpl;\r
 \r
   if ((Token == NULL) || (Token->Event == NULL)) {\r
     return EFI_INVALID_PARAMETER;\r
@@ -2610,7 +3206,11 @@ ScsiDiskAsyncReadSectors (
   }\r
 \r
   BlkIo2Req->Token  = Token;\r
-  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+  InsertTailList (&ScsiDiskDevice->AsyncTaskQueue, &BlkIo2Req->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
+\r
   InitializeListHead (&BlkIo2Req->ScsiRWQueue);\r
 \r
   Status            = EFI_SUCCESS;\r
@@ -2682,6 +3282,7 @@ ScsiDiskAsyncReadSectors (
       Status = ScsiDiskAsyncRead10 (\r
                  ScsiDiskDevice,\r
                  Timeout,\r
+                 0,\r
                  PtrBuffer,\r
                  ByteCount,\r
                  (UINT32) Lba,\r
@@ -2693,6 +3294,7 @@ ScsiDiskAsyncReadSectors (
       Status = ScsiDiskAsyncRead16 (\r
                  ScsiDiskDevice,\r
                  Timeout,\r
+                 0,\r
                  PtrBuffer,\r
                  ByteCount,\r
                  Lba,\r
@@ -2703,15 +3305,47 @@ ScsiDiskAsyncReadSectors (
     }\r
     if (EFI_ERROR (Status)) {\r
       //\r
-      // Free the SCSI_BLKIO2_REQUEST structure only when the first SCSI\r
-      // command fails. Otherwise, it will be freed in the callback function\r
-      // ScsiDiskNotify().\r
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data\r
+      // length of a SCSI I/O command is too large.\r
+      // In this case, we retry sending the SCSI command with a data length\r
+      // half of its previous value.\r
       //\r
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {\r
+        if ((MaxBlock > 1) && (SectorCount > 1)) {\r
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;\r
+          continue;\r
+        }\r
+      }\r
+\r
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
       if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {\r
+        //\r
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other\r
+        // SCSI sub-task running. Otherwise, it will be freed in the callback\r
+        // function ScsiDiskNotify().\r
+        //\r
         RemoveEntryList (&BlkIo2Req->Link);\r
         FreePool (BlkIo2Req);\r
+        BlkIo2Req = NULL;\r
+        gBS->RestoreTPL (OldTpl);\r
+\r
+        //\r
+        // It is safe to return error status to the caller, since there is no\r
+        // previous SCSI sub-task executing.\r
+        //\r
+        Status = EFI_DEVICE_ERROR;\r
+        goto Done;\r
+      } else {\r
+        gBS->RestoreTPL (OldTpl);\r
+\r
+        //\r
+        // There are previous SCSI commands still running, EFI_SUCCESS should\r
+        // be returned to make sure that the caller does not free resources\r
+        // still using by these SCSI commands.\r
+        //\r
+        Status = EFI_SUCCESS;\r
+        goto Done;\r
       }\r
-      return EFI_DEVICE_ERROR;\r
     }\r
 \r
     //\r
@@ -2724,7 +3358,24 @@ ScsiDiskAsyncReadSectors (
     BlocksRemaining -= SectorCount;\r
   }\r
 \r
-  return EFI_SUCCESS;\r
+  Status = EFI_SUCCESS;\r
+\r
+Done:\r
+  if (BlkIo2Req != NULL) {\r
+    BlkIo2Req->LastScsiRW = TRUE;\r
+\r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {\r
+      RemoveEntryList (&BlkIo2Req->Link);\r
+      FreePool (BlkIo2Req);\r
+      BlkIo2Req = NULL;\r
+\r
+      gBS->SignalEvent (Token->Event);\r
+    }\r
+    gBS->RestoreTPL (OldTpl);\r
+  }\r
+\r
+  return Status;\r
 }\r
 \r
 /**\r
@@ -2760,6 +3411,7 @@ ScsiDiskAsyncWriteSectors (
   UINT64                Timeout;\r
   SCSI_BLKIO2_REQUEST   *BlkIo2Req;\r
   EFI_STATUS            Status;\r
+  EFI_TPL               OldTpl;\r
 \r
   if ((Token == NULL) || (Token->Event == NULL)) {\r
     return EFI_INVALID_PARAMETER;\r
@@ -2771,7 +3423,11 @@ ScsiDiskAsyncWriteSectors (
   }\r
 \r
   BlkIo2Req->Token  = Token;\r
-  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+  InsertTailList (&ScsiDiskDevice->AsyncTaskQueue, &BlkIo2Req->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
+\r
   InitializeListHead (&BlkIo2Req->ScsiRWQueue);\r
 \r
   Status            = EFI_SUCCESS;\r
@@ -2843,6 +3499,7 @@ ScsiDiskAsyncWriteSectors (
       Status = ScsiDiskAsyncWrite10 (\r
                  ScsiDiskDevice,\r
                  Timeout,\r
+                 0,\r
                  PtrBuffer,\r
                  ByteCount,\r
                  (UINT32) Lba,\r
@@ -2854,6 +3511,7 @@ ScsiDiskAsyncWriteSectors (
       Status = ScsiDiskAsyncWrite16 (\r
                  ScsiDiskDevice,\r
                  Timeout,\r
+                 0,\r
                  PtrBuffer,\r
                  ByteCount,\r
                  Lba,\r
@@ -2864,15 +3522,47 @@ ScsiDiskAsyncWriteSectors (
     }\r
     if (EFI_ERROR (Status)) {\r
       //\r
-      // Free the SCSI_BLKIO2_REQUEST structure only when the first SCSI\r
-      // command fails. Otherwise, it will be freed in the callback function\r
-      // ScsiDiskNotify().\r
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data\r
+      // length of a SCSI I/O command is too large.\r
+      // In this case, we retry sending the SCSI command with a data length\r
+      // half of its previous value.\r
       //\r
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {\r
+        if ((MaxBlock > 1) && (SectorCount > 1)) {\r
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;\r
+          continue;\r
+        }\r
+      }\r
+\r
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
       if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {\r
+        //\r
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other\r
+        // SCSI sub-task running. Otherwise, it will be freed in the callback\r
+        // function ScsiDiskNotify().\r
+        //\r
         RemoveEntryList (&BlkIo2Req->Link);\r
         FreePool (BlkIo2Req);\r
+        BlkIo2Req = NULL;\r
+        gBS->RestoreTPL (OldTpl);\r
+\r
+        //\r
+        // It is safe to return error status to the caller, since there is no\r
+        // previous SCSI sub-task executing.\r
+        //\r
+        Status = EFI_DEVICE_ERROR;\r
+        goto Done;\r
+      } else {\r
+        gBS->RestoreTPL (OldTpl);\r
+\r
+        //\r
+        // There are previous SCSI commands still running, EFI_SUCCESS should\r
+        // be returned to make sure that the caller does not free resources\r
+        // still using by these SCSI commands.\r
+        //\r
+        Status = EFI_SUCCESS;\r
+        goto Done;\r
       }\r
-      return EFI_DEVICE_ERROR;\r
     }\r
 \r
     //\r
@@ -2885,7 +3575,24 @@ ScsiDiskAsyncWriteSectors (
     BlocksRemaining -= SectorCount;\r
   }\r
 \r
-  return EFI_SUCCESS;\r
+  Status = EFI_SUCCESS;\r
+\r
+Done:\r
+  if (BlkIo2Req != NULL) {\r
+    BlkIo2Req->LastScsiRW = TRUE;\r
+\r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {\r
+      RemoveEntryList (&BlkIo2Req->Link);\r
+      FreePool (BlkIo2Req);\r
+      BlkIo2Req = NULL;\r
+\r
+      gBS->SignalEvent (Token->Event);\r
+    }\r
+    gBS->RestoreTPL (OldTpl);\r
+  }\r
+\r
+  return Status;\r
 }\r
 \r
 \r
@@ -3517,6 +4224,7 @@ Retry:
       Status = ScsiDiskAsyncRead10 (\r
                  ScsiDiskDevice,\r
                  Request->Timeout,\r
+                 Request->TimesRetry,\r
                  Request->InBuffer,\r
                  Request->DataLength,\r
                  (UINT32) Request->StartLba,\r
@@ -3528,6 +4236,7 @@ Retry:
       Status = ScsiDiskAsyncRead16 (\r
                  ScsiDiskDevice,\r
                  Request->Timeout,\r
+                 Request->TimesRetry,\r
                  Request->InBuffer,\r
                  Request->DataLength,\r
                  Request->StartLba,\r
@@ -3549,6 +4258,7 @@ Retry:
         Status = ScsiDiskAsyncRead10 (\r
                    ScsiDiskDevice,\r
                    Request->Timeout,\r
+                   0,\r
                    Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,\r
                    OldDataLength - Request->DataLength,\r
                    (UINT32) Request->StartLba + Request->SectorCount,\r
@@ -3560,6 +4270,7 @@ Retry:
         Status = ScsiDiskAsyncRead16 (\r
                    ScsiDiskDevice,\r
                    Request->Timeout,\r
+                   0,\r
                    Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,\r
                    OldDataLength - Request->DataLength,\r
                    Request->StartLba + Request->SectorCount,\r
@@ -3581,6 +4292,7 @@ Retry:
       Status = ScsiDiskAsyncWrite10 (\r
                  ScsiDiskDevice,\r
                  Request->Timeout,\r
+                 Request->TimesRetry,\r
                  Request->OutBuffer,\r
                  Request->DataLength,\r
                  (UINT32) Request->StartLba,\r
@@ -3592,6 +4304,7 @@ Retry:
       Status = ScsiDiskAsyncWrite16 (\r
                  ScsiDiskDevice,\r
                  Request->Timeout,\r
+                 Request->TimesRetry,\r
                  Request->OutBuffer,\r
                  Request->DataLength,\r
                  Request->StartLba,\r
@@ -3613,6 +4326,7 @@ Retry:
         Status = ScsiDiskAsyncWrite10 (\r
                    ScsiDiskDevice,\r
                    Request->Timeout,\r
+                   0,\r
                    Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,\r
                    OldDataLength - Request->DataLength,\r
                    (UINT32) Request->StartLba + Request->SectorCount,\r
@@ -3624,6 +4338,7 @@ Retry:
         Status = ScsiDiskAsyncWrite16 (\r
                    ScsiDiskDevice,\r
                    Request->Timeout,\r
+                   0,\r
                    Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,\r
                    OldDataLength - Request->DataLength,\r
                    Request->StartLba + Request->SectorCount,\r
@@ -3641,7 +4356,8 @@ Retry:
 \r
 Exit:\r
   RemoveEntryList (&Request->Link);\r
-  if (IsListEmpty (&Request->BlkIo2Req->ScsiRWQueue)) {\r
+  if ((IsListEmpty (&Request->BlkIo2Req->ScsiRWQueue)) &&\r
+      (Request->BlkIo2Req->LastScsiRW)) {\r
     //\r
     // The last SCSI R/W command of a BlockIo2 request completes\r
     //\r
@@ -3660,6 +4376,7 @@ Exit:
 \r
   @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.\r
   @param  Timeout            The time to complete the command.\r
+  @param  TimesRetry         The number of times the command has been retried.\r
   @param  DataBuffer         The buffer to fill with the read out data.\r
   @param  DataLength         The length of buffer.\r
   @param  StartLba           The start logic block address.\r
@@ -3678,6 +4395,7 @@ EFI_STATUS
 ScsiDiskAsyncRead10 (\r
   IN     SCSI_DISK_DEV         *ScsiDiskDevice,\r
   IN     UINT64                Timeout,\r
+  IN     UINT8                 TimesRetry,\r
      OUT UINT8                 *DataBuffer,\r
   IN     UINT32                DataLength,\r
   IN     UINT32                StartLba,\r
@@ -3689,12 +4407,18 @@ ScsiDiskAsyncRead10 (
   EFI_STATUS                   Status;\r
   SCSI_ASYNC_RW_REQUEST        *Request;\r
   EFI_EVENT                    AsyncIoEvent;\r
+  EFI_TPL                      OldTpl;\r
+\r
+  AsyncIoEvent = NULL;\r
 \r
   Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));\r
   if (Request == NULL) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
   InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
 \r
   Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));\r
   Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);\r
@@ -3705,6 +4429,7 @@ ScsiDiskAsyncRead10 (
 \r
   Request->ScsiDiskDevice  = ScsiDiskDevice;\r
   Request->Timeout         = Timeout;\r
+  Request->TimesRetry      = TimesRetry;\r
   Request->InBuffer        = DataBuffer;\r
   Request->DataLength      = DataLength;\r
   Request->StartLba        = StartLba;\r
@@ -3716,7 +4441,7 @@ ScsiDiskAsyncRead10 (
   //\r
   Status = gBS->CreateEvent (\r
                   EVT_NOTIFY_SIGNAL,\r
-                  TPL_CALLBACK,\r
+                  TPL_NOTIFY,\r
                   ScsiDiskNotify,\r
                   Request,\r
                   &AsyncIoEvent\r
@@ -3745,12 +4470,19 @@ ScsiDiskAsyncRead10 (
   return EFI_SUCCESS;\r
 \r
 ErrorExit:\r
+  if (AsyncIoEvent != NULL) {\r
+    gBS->CloseEvent (AsyncIoEvent);\r
+  }\r
+\r
   if (Request != NULL) {\r
     if (Request->SenseData != NULL) {\r
       FreePool (Request->SenseData);\r
     }\r
 \r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
     RemoveEntryList (&Request->Link);\r
+    gBS->RestoreTPL (OldTpl);\r
+\r
     FreePool (Request);\r
   }\r
 \r
@@ -3763,6 +4495,7 @@ ErrorExit:
 \r
   @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.\r
   @param  Timeout            The time to complete the command.\r
+  @param  TimesRetry         The number of times the command has been retried.\r
   @param  DataBuffer         The buffer contains the data to write.\r
   @param  DataLength         The length of buffer.\r
   @param  StartLba           The start logic block address.\r
@@ -3781,6 +4514,7 @@ EFI_STATUS
 ScsiDiskAsyncWrite10 (\r
   IN     SCSI_DISK_DEV         *ScsiDiskDevice,\r
   IN     UINT64                Timeout,\r
+  IN     UINT8                 TimesRetry,\r
   IN     UINT8                 *DataBuffer,\r
   IN     UINT32                DataLength,\r
   IN     UINT32                StartLba,\r
@@ -3792,12 +4526,18 @@ ScsiDiskAsyncWrite10 (
   EFI_STATUS                   Status;\r
   SCSI_ASYNC_RW_REQUEST        *Request;\r
   EFI_EVENT                    AsyncIoEvent;\r
+  EFI_TPL                      OldTpl;\r
+\r
+  AsyncIoEvent = NULL;\r
 \r
   Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));\r
   if (Request == NULL) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
   InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
 \r
   Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));\r
   Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);\r
@@ -3808,6 +4548,7 @@ ScsiDiskAsyncWrite10 (
 \r
   Request->ScsiDiskDevice  = ScsiDiskDevice;\r
   Request->Timeout         = Timeout;\r
+  Request->TimesRetry      = TimesRetry;\r
   Request->OutBuffer       = DataBuffer;\r
   Request->DataLength      = DataLength;\r
   Request->StartLba        = StartLba;\r
@@ -3819,7 +4560,7 @@ ScsiDiskAsyncWrite10 (
   //\r
   Status = gBS->CreateEvent (\r
                   EVT_NOTIFY_SIGNAL,\r
-                  TPL_CALLBACK,\r
+                  TPL_NOTIFY,\r
                   ScsiDiskNotify,\r
                   Request,\r
                   &AsyncIoEvent\r
@@ -3848,12 +4589,19 @@ ScsiDiskAsyncWrite10 (
   return EFI_SUCCESS;\r
 \r
 ErrorExit:\r
+  if (AsyncIoEvent != NULL) {\r
+    gBS->CloseEvent (AsyncIoEvent);\r
+  }\r
+\r
   if (Request != NULL) {\r
     if (Request->SenseData != NULL) {\r
       FreePool (Request->SenseData);\r
     }\r
 \r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
     RemoveEntryList (&Request->Link);\r
+    gBS->RestoreTPL (OldTpl);\r
+\r
     FreePool (Request);\r
   }\r
 \r
@@ -3866,6 +4614,7 @@ ErrorExit:
 \r
   @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.\r
   @param  Timeout            The time to complete the command.\r
+  @param  TimesRetry         The number of times the command has been retried.\r
   @param  DataBuffer         The buffer to fill with the read out data.\r
   @param  DataLength         The length of buffer.\r
   @param  StartLba           The start logic block address.\r
@@ -3884,6 +4633,7 @@ EFI_STATUS
 ScsiDiskAsyncRead16 (\r
   IN     SCSI_DISK_DEV         *ScsiDiskDevice,\r
   IN     UINT64                Timeout,\r
+  IN     UINT8                 TimesRetry,\r
      OUT UINT8                 *DataBuffer,\r
   IN     UINT32                DataLength,\r
   IN     UINT64                StartLba,\r
@@ -3895,12 +4645,18 @@ ScsiDiskAsyncRead16 (
   EFI_STATUS                   Status;\r
   SCSI_ASYNC_RW_REQUEST        *Request;\r
   EFI_EVENT                    AsyncIoEvent;\r
+  EFI_TPL                      OldTpl;\r
+\r
+  AsyncIoEvent = NULL;\r
 \r
   Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));\r
   if (Request == NULL) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
   InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
 \r
   Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));\r
   Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);\r
@@ -3911,6 +4667,7 @@ ScsiDiskAsyncRead16 (
 \r
   Request->ScsiDiskDevice  = ScsiDiskDevice;\r
   Request->Timeout         = Timeout;\r
+  Request->TimesRetry      = TimesRetry;\r
   Request->InBuffer        = DataBuffer;\r
   Request->DataLength      = DataLength;\r
   Request->StartLba        = StartLba;\r
@@ -3922,7 +4679,7 @@ ScsiDiskAsyncRead16 (
   //\r
   Status = gBS->CreateEvent (\r
                   EVT_NOTIFY_SIGNAL,\r
-                  TPL_CALLBACK,\r
+                  TPL_NOTIFY,\r
                   ScsiDiskNotify,\r
                   Request,\r
                   &AsyncIoEvent\r
@@ -3951,12 +4708,19 @@ ScsiDiskAsyncRead16 (
   return EFI_SUCCESS;\r
 \r
 ErrorExit:\r
+  if (AsyncIoEvent != NULL) {\r
+    gBS->CloseEvent (AsyncIoEvent);\r
+  }\r
+\r
   if (Request != NULL) {\r
     if (Request->SenseData != NULL) {\r
       FreePool (Request->SenseData);\r
     }\r
 \r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
     RemoveEntryList (&Request->Link);\r
+    gBS->RestoreTPL (OldTpl);\r
+\r
     FreePool (Request);\r
   }\r
 \r
@@ -3969,6 +4733,7 @@ ErrorExit:
 \r
   @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.\r
   @param  Timeout            The time to complete the command.\r
+  @param  TimesRetry         The number of times the command has been retried.\r
   @param  DataBuffer         The buffer contains the data to write.\r
   @param  DataLength         The length of buffer.\r
   @param  StartLba           The start logic block address.\r
@@ -3987,6 +4752,7 @@ EFI_STATUS
 ScsiDiskAsyncWrite16 (\r
   IN     SCSI_DISK_DEV         *ScsiDiskDevice,\r
   IN     UINT64                Timeout,\r
+  IN     UINT8                 TimesRetry,\r
   IN     UINT8                 *DataBuffer,\r
   IN     UINT32                DataLength,\r
   IN     UINT64                StartLba,\r
@@ -3998,12 +4764,18 @@ ScsiDiskAsyncWrite16 (
   EFI_STATUS                   Status;\r
   SCSI_ASYNC_RW_REQUEST        *Request;\r
   EFI_EVENT                    AsyncIoEvent;\r
+  EFI_TPL                      OldTpl;\r
+\r
+  AsyncIoEvent = NULL;\r
 \r
   Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));\r
   if (Request == NULL) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
+\r
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
   InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);\r
+  gBS->RestoreTPL (OldTpl);\r
 \r
   Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));\r
   Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);\r
@@ -4014,6 +4786,7 @@ ScsiDiskAsyncWrite16 (
 \r
   Request->ScsiDiskDevice  = ScsiDiskDevice;\r
   Request->Timeout         = Timeout;\r
+  Request->TimesRetry      = TimesRetry;\r
   Request->OutBuffer       = DataBuffer;\r
   Request->DataLength      = DataLength;\r
   Request->StartLba        = StartLba;\r
@@ -4025,7 +4798,7 @@ ScsiDiskAsyncWrite16 (
   //\r
   Status = gBS->CreateEvent (\r
                   EVT_NOTIFY_SIGNAL,\r
-                  TPL_CALLBACK,\r
+                  TPL_NOTIFY,\r
                   ScsiDiskNotify,\r
                   Request,\r
                   &AsyncIoEvent\r
@@ -4054,12 +4827,19 @@ ScsiDiskAsyncWrite16 (
   return EFI_SUCCESS;\r
 \r
 ErrorExit:\r
+  if (AsyncIoEvent != NULL) {\r
+    gBS->CloseEvent (AsyncIoEvent);\r
+  }\r
+\r
   if (Request != NULL) {\r
     if (Request->SenseData != NULL) {\r
       FreePool (Request->SenseData);\r
     }\r
 \r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
     RemoveEntryList (&Request->Link);\r
+    gBS->RestoreTPL (OldTpl);\r
+\r
     FreePool (Request);\r
   }\r
 \r
@@ -4214,7 +4994,7 @@ ScsiDiskIsHardwareError (
   SensePtr  = SenseData;\r
 \r
   for (Index = 0; Index < SenseCounts; Index++) {\r
-    \r
+\r
     //\r
     // Sense Key is EFI_SCSI_SK_HARDWARE_ERROR (0x4)\r
     //\r
@@ -4291,7 +5071,7 @@ ScsiDiskIsResetBefore (
   SensePtr      = SenseData;\r
 \r
   for (Index = 0; Index < SenseCounts; Index++) {\r
-    \r
+\r
     //\r
     // Sense Key is EFI_SCSI_SK_UNIT_ATTENTION (0x6)\r
     // Additional Sense Code is EFI_SCSI_ASC_RESET (0x29)\r
@@ -4312,7 +5092,7 @@ ScsiDiskIsResetBefore (
 \r
   @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA\r
   @param  SenseCounts  The number of sense key\r
-  @param  RetryLater   The flag means if need a retry \r
+  @param  RetryLater   The flag means if need a retry\r
 \r
   @retval TRUE  Drive is ready.\r
   @retval FALSE Drive is NOT ready.\r
@@ -4407,7 +5187,7 @@ ScsiDiskHaveSenseKey (
   SensePtr = SenseData;\r
 \r
   for (Index = 0; Index < SenseCounts; Index++) {\r
-    \r
+\r
     //\r
     // Sense Key is SK_NO_SENSE (0x0)\r
     //\r
@@ -4454,18 +5234,18 @@ ReleaseScsiDiskDeviceResources (
 \r
 /**\r
   Determine if Block Io & Block Io2 should be produced.\r
-  \r
+\r
 \r
   @param  ChildHandle  Child Handle to retrieve Parent information.\r
-  \r
+\r
   @retval  TRUE    Should produce Block Io & Block Io2.\r
   @retval  FALSE   Should not produce Block Io & Block Io2.\r
 \r
-**/  \r
+**/\r
 BOOLEAN\r
 DetermineInstallBlockIo (\r
   IN  EFI_HANDLE      ChildHandle\r
-  )  \r
+  )\r
 {\r
   EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiPassThru;\r
   EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiPassThru;\r
@@ -4491,7 +5271,7 @@ DetermineInstallBlockIo (
       return TRUE;\r
     }\r
   }\r
-  \r
+\r
   return FALSE;\r
 }\r
 \r
@@ -4500,23 +5280,23 @@ DetermineInstallBlockIo (
   specified by ProtocolGuid is present on a ControllerHandle and opened by\r
   ChildHandle with an attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.\r
   If the ControllerHandle is found, then the protocol specified by ProtocolGuid\r
-  will be opened on it.  \r
-  \r
+  will be opened on it.\r
+\r
 \r
   @param  ProtocolGuid   ProtocolGuid pointer.\r
   @param  ChildHandle    Child Handle to retrieve Parent information.\r
-  \r
-**/ \r
+\r
+**/\r
 VOID *\r
 EFIAPI\r
 GetParentProtocol (\r
   IN  EFI_GUID                          *ProtocolGuid,\r
   IN  EFI_HANDLE                        ChildHandle\r
-  ) \r
+  )\r
 {\r
   UINTN                                 Index;\r
   UINTN                                 HandleCount;\r
-  VOID                                  *Interface;  \r
+  VOID                                  *Interface;\r
   EFI_STATUS                            Status;\r
   EFI_HANDLE                            *HandleBuffer;\r
 \r
@@ -4536,7 +5316,7 @@ GetParentProtocol (
   }\r
 \r
   //\r
-  // Iterate to find who is parent handle that is opened with ProtocolGuid by ChildHandle \r
+  // Iterate to find who is parent handle that is opened with ProtocolGuid by ChildHandle\r
   //\r
   for (Index = 0; Index < HandleCount; Index++) {\r
     Status = EfiTestChildHandle (HandleBuffer[Index], ChildHandle, ProtocolGuid);\r
@@ -4551,11 +5331,144 @@ GetParentProtocol (
 \r
   gBS->FreePool (HandleBuffer);\r
   return NULL;\r
-} \r
+}\r
+\r
+/**\r
+  Determine if EFI Erase Block Protocol should be produced.\r
+\r
+  @param   ScsiDiskDevice    The pointer of SCSI_DISK_DEV.\r
+  @param   ChildHandle       Handle of device.\r
+\r
+  @retval  TRUE    Should produce EFI Erase Block Protocol.\r
+  @retval  FALSE   Should not produce EFI Erase Block Protocol.\r
+\r
+**/\r
+BOOLEAN\r
+DetermineInstallEraseBlock (\r
+  IN  SCSI_DISK_DEV          *ScsiDiskDevice,\r
+  IN  EFI_HANDLE             ChildHandle\r
+  )\r
+{\r
+  UINT8                           HostAdapterStatus;\r
+  UINT8                           TargetStatus;\r
+  EFI_STATUS                      CommandStatus;\r
+  EFI_STATUS                      Status;\r
+  BOOLEAN                         UfsDevice;\r
+  BOOLEAN                         RetVal;\r
+  EFI_DEVICE_PATH_PROTOCOL        *DevicePathNode;\r
+  UINT8                           SenseDataLength;\r
+  UINT32                          DataLength16;\r
+  EFI_SCSI_DISK_CAPACITY_DATA16   *CapacityData16;\r
+\r
+  UfsDevice      = FALSE;\r
+  RetVal         = TRUE;\r
+  CapacityData16 = NULL;\r
+\r
+  Status = gBS->HandleProtocol (\r
+                  ChildHandle,\r
+                  &gEfiDevicePathProtocolGuid,\r
+                  (VOID **) &DevicePathNode\r
+                  );\r
+  //\r
+  // Device Path protocol must be installed on the device handle.\r
+  //\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  while (!IsDevicePathEndType (DevicePathNode)) {\r
+    //\r
+    // For now, only support Erase Block Protocol on UFS devices.\r
+    //\r
+    if ((DevicePathNode->Type == MESSAGING_DEVICE_PATH) &&\r
+        (DevicePathNode->SubType == MSG_UFS_DP)) {\r
+      UfsDevice = TRUE;\r
+      break;\r
+    }\r
+\r
+    DevicePathNode = NextDevicePathNode (DevicePathNode);\r
+  }\r
+  if (!UfsDevice) {\r
+    RetVal = FALSE;\r
+    goto Done;\r
+  }\r
+\r
+  //\r
+  // Check whether the erase functionality is enabled on the UFS device.\r
+  //\r
+  CapacityData16 = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));\r
+  if (CapacityData16 == NULL) {\r
+    RetVal = FALSE;\r
+    goto Done;\r
+  }\r
+\r
+  SenseDataLength = 0;\r
+  DataLength16    = sizeof (EFI_SCSI_DISK_CAPACITY_DATA16);\r
+  ZeroMem (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));\r
+\r
+  CommandStatus = ScsiReadCapacity16Command (\r
+                    ScsiDiskDevice->ScsiIo,\r
+                    SCSI_DISK_TIMEOUT,\r
+                    NULL,\r
+                    &SenseDataLength,\r
+                    &HostAdapterStatus,\r
+                    &TargetStatus,\r
+                    (VOID *) CapacityData16,\r
+                    &DataLength16,\r
+                    FALSE\r
+                    );\r
+\r
+  if (CommandStatus == EFI_SUCCESS) {\r
+    //\r
+    // Universal Flash Storage (UFS) Version 2.0\r
+    // Section 11.3.9.2\r
+    // Bits TPE and TPRZ should both be set to enable the erase feature on UFS.\r
+    //\r
+    if (((CapacityData16->LowestAlignLogic2 & BIT7) == 0) ||\r
+        ((CapacityData16->LowestAlignLogic2 & BIT6) == 0)) {\r
+      DEBUG ((\r
+        EFI_D_VERBOSE,\r
+        "ScsiDisk EraseBlock: Either TPE or TPRZ is not set: 0x%x.\n",\r
+        CapacityData16->LowestAlignLogic2\r
+        ));\r
+\r
+      RetVal = FALSE;\r
+      goto Done;\r
+    }\r
+  } else {\r
+    DEBUG ((\r
+      EFI_D_VERBOSE,\r
+      "ScsiDisk EraseBlock: ReadCapacity16 failed with status %r.\n",\r
+      CommandStatus\r
+      ));\r
+\r
+    RetVal = FALSE;\r
+    goto Done;\r
+  }\r
+\r
+  //\r
+  // Check whether the UFS device server implements the UNMAP command.\r
+  //\r
+  if ((ScsiDiskDevice->UnmapInfo.MaxLbaCnt == 0) ||\r
+      (ScsiDiskDevice->UnmapInfo.MaxBlkDespCnt == 0)) {\r
+    DEBUG ((\r
+      EFI_D_VERBOSE,\r
+      "ScsiDisk EraseBlock: The device server does not implement the UNMAP command.\n"\r
+      ));\r
+\r
+    RetVal = FALSE;\r
+    goto Done;\r
+  }\r
+\r
+Done:\r
+  if (CapacityData16 != NULL) {\r
+    FreeAlignedBuffer (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));\r
+  }\r
+\r
+  return RetVal;\r
+}\r
 \r
 /**\r
   Provides inquiry information for the controller type.\r
-  \r
+\r
   This function is used by the IDE bus driver to get inquiry data.  Data format\r
   of Identify data is defined by the Interface GUID.\r
 \r
@@ -4564,9 +5477,9 @@ GetParentProtocol (
   @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.\r
 \r
   @retval EFI_SUCCESS            The command was accepted without any errors.\r
-  @retval EFI_NOT_FOUND          Device does not support this data class \r
-  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device \r
-  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough \r
+  @retval EFI_NOT_FOUND          Device does not support this data class\r
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device\r
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough\r
 \r
 **/\r
 EFI_STATUS\r
@@ -4598,16 +5511,16 @@ ScsiDiskInfoInquiry (
   This function is used by the IDE bus driver to get identify data.  Data format\r
   of Identify data is defined by the Interface GUID.\r
 \r
-  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL \r
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL\r
                                     instance.\r
   @param[in, out] IdentifyData      Pointer to a buffer for the identify data.\r
   @param[in, out] IdentifyDataSize  Pointer to the value for the identify data\r
                                     size.\r
 \r
   @retval EFI_SUCCESS            The command was accepted without any errors.\r
-  @retval EFI_NOT_FOUND          Device does not support this data class \r
-  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device \r
-  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough \r
+  @retval EFI_NOT_FOUND          Device does not support this data class\r
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device\r
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough\r
 \r
 **/\r
 EFI_STATUS\r
@@ -4623,7 +5536,7 @@ ScsiDiskInfoIdentify (
 \r
   if (CompareGuid (&This->Interface, &gEfiDiskInfoScsiInterfaceGuid) || CompareGuid (&This->Interface, &gEfiDiskInfoUfsInterfaceGuid)) {\r
     //\r
-    // Physical SCSI bus does not support this data class. \r
+    // Physical SCSI bus does not support this data class.\r
     //\r
     return EFI_NOT_FOUND;\r
   }\r
@@ -4641,8 +5554,8 @@ ScsiDiskInfoIdentify (
 \r
 /**\r
   Provides sense data information for the controller type.\r
-  \r
-  This function is used by the IDE bus driver to get sense data. \r
+\r
+  This function is used by the IDE bus driver to get sense data.\r
   Data format of Sense data is defined by the Interface GUID.\r
 \r
   @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.\r
@@ -4672,7 +5585,7 @@ ScsiDiskInfoSenseData (
 /**\r
   This function is used by the IDE bus driver to get controller information.\r
 \r
-  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. \r
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.\r
   @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.\r
   @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.\r
 \r
@@ -4713,11 +5626,11 @@ ScsiDiskInfoWhichIde (
   via SCSI Request Packet.\r
 \r
   @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV\r
-  \r
+\r
   @retval EFI_SUCCESS     The ATAPI device identify data were retrieved successfully.\r
   @retval others          Some error occurred during the identification that ATAPI device.\r
 \r
-**/  \r
+**/\r
 EFI_STATUS\r
 AtapiIdentifyDevice (\r
   IN OUT SCSI_DISK_DEV   *ScsiDiskDevice\r
@@ -4753,8 +5666,8 @@ AtapiIdentifyDevice (
 \r
   @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.\r
   @param  ChildHandle     Child handle to install DiskInfo protocol.\r
-  \r
-**/  \r
+\r
+**/\r
 VOID\r
 InitializeInstallDiskInfo (\r
   IN  SCSI_DISK_DEV   *ScsiDiskDevice,\r
@@ -4770,7 +5683,7 @@ InitializeInstallDiskInfo (
 \r
   Status = gBS->HandleProtocol (ChildHandle, &gEfiDevicePathProtocolGuid, (VOID **) &DevicePathNode);\r
   //\r
-  // Device Path protocol must be installed on the device handle. \r
+  // Device Path protocol must be installed on the device handle.\r
   //\r
   ASSERT_EFI_ERROR (Status);\r
   //\r
@@ -4802,7 +5715,7 @@ InitializeInstallDiskInfo (
             ScsiDiskDevice->Channel = AtapiDevicePath->PrimarySecondary;\r
             ScsiDiskDevice->Device = AtapiDevicePath->SlaveMaster;\r
             //\r
-            // Update the DiskInfo.Interface to IDE interface GUID for the physical ATAPI device. \r
+            // Update the DiskInfo.Interface to IDE interface GUID for the physical ATAPI device.\r
             //\r
             CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoIdeInterfaceGuid);\r
           } else {\r
@@ -4813,7 +5726,7 @@ InitializeInstallDiskInfo (
             ScsiDiskDevice->Channel = SataDevicePath->HBAPortNumber;\r
             ScsiDiskDevice->Device = SataDevicePath->PortMultiplierPortNumber;\r
             //\r
-            // Update the DiskInfo.Interface to AHCI interface GUID for the physical AHCI device. \r
+            // Update the DiskInfo.Interface to AHCI interface GUID for the physical AHCI device.\r
             //\r
             CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoAhciInterfaceGuid);\r
           }\r