Fix a bug about unaligned read in UEFI mode.

[Ventoy.git] / EDK2 / edk2_mod / edk2-edk2-stable201911 / MdeModulePkg / Application / Ventoy / VentoyProtocol.c

diff --git a/EDK2/edk2_mod/edk2-edk2-stable201911/MdeModulePkg/Application/Ventoy/VentoyProtocol.c b/EDK2/edk2_mod/edk2-edk2-stable201911/MdeModulePkg/Application/Ventoy/VentoyProtocol.c
index c8b798438247eac3c1800d6bd36079b0d6c3ef76..f9323cb933e5a86442e582e21a2540d9a257ba5a 100644 (file)
--- a/EDK2/edk2_mod/edk2-edk2-stable201911/MdeModulePkg/Application/Ventoy/VentoyProtocol.c
+++ b/EDK2/edk2_mod/edk2-edk2-stable201911/MdeModulePkg/Application/Ventoy/VentoyProtocol.c
@@ -148,7 +148,7 @@ STATIC EFI_STATUS EFIAPI ventoy_read_iso_sector
     ventoy_override_chunk *pOverride = g_override_chunk;
     EFI_BLOCK_IO_PROTOCOL *pRawBlockIo = gBlockData.pRawBlockIo;
     
-    debug("read iso sector %lu  count %u", Sector, Count);
+    debug("read iso sector %lu count %u Buffer:%p Align:%u", Sector, Count, Buffer, pRawBlockIo->Media->IoAlign);
 
     ReadStart = Sector * 2048;
     ReadEnd = (Sector + Count) * 2048;
@@ -173,7 +173,6 @@ STATIC EFI_STATUS EFIAPI ventoy_read_iso_sector
             {
                 MapLba = ((Sector - pchunk->img_start_sector) >> 1) + pchunk->disk_start_sector;
             }
-            
 
             secLeft = pchunk->img_end_sector + 1 - Sector;
             secRead = (Count < secLeft) ? Count : secLeft;
@@ -182,7 +181,7 @@ STATIC EFI_STATUS EFIAPI ventoy_read_iso_sector
                                      MapLba, secRead * 2048, pCurBuf);
             if (EFI_ERROR(Status))
             {
-                debug("Raw disk read block failed %r LBA:%lu Count:%u", Status, MapLba, secRead);
+                debug("Raw disk read block failed %r LBA:%lu Count:%u %p", Status, MapLba, secRead, pCurBuf);
                 return Status;
             }
 
@@ -425,7 +424,7 @@ end:
     return Lba;
 }
 
-EFI_STATUS EFIAPI ventoy_block_io_read 
+EFI_STATUS EFIAPI ventoy_block_io_read_real 
 (
     IN EFI_BLOCK_IO_PROTOCOL          *This,
     IN UINT32                          MediaId,
@@ -438,6 +437,8 @@ EFI_STATUS EFIAPI ventoy_block_io_read
     UINT32 j = 0;
     UINT32 lbacount = 0;
     UINT32 secNum = 0;
+    UINT32 TmpNum = 0;
+    UINT64 VirtSec = 0;
     UINT64 offset = 0;
     EFI_LBA curlba = 0;
     EFI_LBA lastlba = 0;
@@ -445,7 +446,7 @@ EFI_STATUS EFIAPI ventoy_block_io_read
     ventoy_sector_flag *cur_flag;
     ventoy_virt_chunk *node;
     
-    //debug("### ventoy_block_io_read sector:%u count:%u", (UINT32)Lba, (UINT32)BufferSize / 2048);
+    debug("### block_io_read_real sector:%u count:%u Buffer:%p", (UINT32)Lba, (UINT32)BufferSize / 2048, Buffer);
 
     secNum = BufferSize / 2048;
 
@@ -461,6 +462,26 @@ EFI_STATUS EFIAPI ventoy_block_io_read
     {
         return ventoy_read_iso_sector(Lba, secNum, Buffer);
     }
+    else if (offset < g_chain->real_img_size_in_bytes)
+    {
+        TmpNum = (g_chain->real_img_size_in_bytes - offset) / 2048;
+        ventoy_read_iso_sector(Lba, TmpNum, Buffer);
+
+        Lba += TmpNum;
+        secNum -= TmpNum;
+        Buffer = (UINT8 *)Buffer + (g_chain->real_img_size_in_bytes - offset);
+        offset = Lba * 2048;
+    }
+
+    VirtSec = g_chain->virt_img_size_in_bytes / 2048;
+    if (Lba >= VirtSec)
+    {
+        return EFI_SUCCESS;
+    }
+    else if (Lba + secNum > VirtSec)
+    {
+        secNum = VirtSec - Lba;
+    }
 
     if (secNum > g_sector_flag_num)
     {
@@ -529,6 +550,42 @@ EFI_STATUS EFIAPI ventoy_block_io_read
        return EFI_SUCCESS;
 }
 
+EFI_STATUS EFIAPI ventoy_block_io_read
+(
+    IN EFI_BLOCK_IO_PROTOCOL          *This,
+    IN UINT32                          MediaId,
+    IN EFI_LBA                         Lba,
+    IN UINTN                           BufferSize,
+    OUT VOID                          *Buffer
+) 
+{
+    UINT32 IoAlign = 0;
+    VOID *NewBuf = NULL;
+    EFI_STATUS Status = EFI_OUT_OF_RESOURCES;
+
+    if (gBlockData.pRawBlockIo && gBlockData.pRawBlockIo->Media)
+    {
+        IoAlign = gBlockData.pRawBlockIo->Media->IoAlign;
+    }
+
+    if ((IoAlign == 0) || (((UINTN) Buffer & (IoAlign - 1)) == 0))
+    {
+        Status = ventoy_block_io_read_real(This, MediaId, Lba, BufferSize, Buffer);
+    }
+    else
+    {
+        NewBuf = AllocatePages(EFI_SIZE_TO_PAGES(BufferSize + IoAlign));
+        if (NewBuf)
+        {
+            Status = ventoy_block_io_read_real(This, MediaId, Lba, BufferSize, NewBuf);
+            CopyMem(Buffer, NewBuf, BufferSize);
+            FreePages(NewBuf, EFI_SIZE_TO_PAGES(BufferSize + IoAlign));
+        }
+    }
+
+    return Status;
+}
+
 EFI_STATUS EFIAPI ventoy_block_io_write 
 (
     IN EFI_BLOCK_IO_PROTOCOL          *This,
@@ -698,7 +755,7 @@ EFI_STATUS EFIAPI ventoy_block_io_read_512
     UINT8 *CurBuf = NULL;
     EFI_STATUS Status = EFI_SUCCESS;
 
-    debug("ventoy_block_io_read_512 %lu %lu\n", Lba, BufferSize / 512);
+    debug("ventoy_block_io_read_512 %lu %lu Buffer:%p\n", Lba, BufferSize / 512, Buffer);
 
     CurBuf = (UINT8 *)Buffer;