+EFI_STATUS EFIAPI ventoy_block_io_read_512
+(
+ IN EFI_BLOCK_IO_PROTOCOL *This,
+ IN UINT32 MediaId,
+ IN EFI_LBA Lba,
+ IN UINTN BufferSize,
+ OUT VOID *Buffer
+)
+{
+ EFI_LBA Mod;
+ UINTN ReadSize;
+ UINT8 *CurBuf = NULL;
+ EFI_STATUS Status = EFI_SUCCESS;
+
+ debug("ventoy_block_io_read_512 %lu %lu Buffer:%p\n", Lba, BufferSize / 512, Buffer);
+
+ CurBuf = (UINT8 *)Buffer;
+
+ Mod = Lba % 4;
+ if (Mod > 0)
+ {
+ Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
+
+ if (BufferSize <= (4 - Mod) * 512)
+ {
+ CopyMem(CurBuf, g_sector_buf + Mod * 512, BufferSize);
+ return EFI_SUCCESS;
+ }
+ else
+ {
+ ReadSize = (4 - Mod) * 512;
+ CopyMem(CurBuf, g_sector_buf + Mod * 512, ReadSize);
+ CurBuf += ReadSize;
+ Lba += (4 - Mod);
+ BufferSize -= ReadSize;
+ }
+ }
+
+ if (BufferSize >= 2048)
+ {
+ ReadSize = BufferSize / 2048 * 2048;
+
+ Status |= g_sector_2048_read(This, MediaId, Lba / 4, ReadSize, CurBuf);
+ CurBuf += ReadSize;
+
+ Lba += ReadSize / 512;
+ BufferSize -= ReadSize;
+ }
+
+ if (BufferSize > 0)
+ {
+ Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
+ CopyMem(CurBuf, g_sector_buf, BufferSize);
+ }
+
+ return Status;
+}
+
+EFI_STATUS EFIAPI ventoy_block_io_write_512
+(
+ IN EFI_BLOCK_IO_PROTOCOL *This,
+ IN UINT32 MediaId,
+ IN EFI_LBA Lba,
+ IN UINTN BufferSize,
+ IN VOID *Buffer
+)
+{
+ EFI_LBA Mod;
+ UINTN ReadSize;
+ UINT8 *CurBuf = NULL;
+ EFI_STATUS Status = EFI_SUCCESS;
+
+ debug("ventoy_block_io_write_512 %lu %lu\n", Lba, BufferSize / 512);
+
+ CurBuf = (UINT8 *)Buffer;
+
+ Mod = Lba % 4;
+ if (Mod > 0)
+ {
+ Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
+
+ if (BufferSize <= (4 - Mod) * 512)
+ {
+ CopyMem(g_sector_buf + Mod * 512, CurBuf, BufferSize);
+ return g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
+ }
+ else
+ {
+ ReadSize = (4 - Mod) * 512;
+ CopyMem(g_sector_buf + Mod * 512, CurBuf, ReadSize);
+ g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
+
+ CurBuf += ReadSize;
+ Lba += (4 - Mod);
+ BufferSize -= ReadSize;
+ }
+ }
+
+ if (BufferSize >= 2048)
+ {
+ ReadSize = BufferSize / 2048 * 2048;
+
+ Status |= g_sector_2048_write(This, MediaId, Lba / 4, ReadSize, CurBuf);
+ CurBuf += ReadSize;
+
+ Lba += ReadSize / 512;
+ BufferSize -= ReadSize;
+ }
+
+ if (BufferSize > 0)
+ {
+ Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
+
+ CopyMem(g_sector_buf, CurBuf, BufferSize);
+ g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
+ }
+
+ return Status;
+}
+