]> glassweightruler.freedombox.rocks Git - Ventoy.git/blob - EDK2/edk2_mod/edk2-edk2-stable201911/MdeModulePkg/Application/Ventoy/VentoyProtocol.c
projects / Ventoy.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
Update README.md
[Ventoy.git] / EDK2 / edk2_mod / edk2-edk2-stable201911 / MdeModulePkg / Application / Ventoy / VentoyProtocol.c
1 /******************************************************************************
2 * Ventoy.c
3 *
4 * Copyright (c) 2020, longpanda <admin@ventoy.net>
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License as
8 * published by the Free Software Foundation; either version 3 of the
9 * License, or (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful, but
12 * WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 * General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, see <http://www.gnu.org/licenses/>.
18 *
19 */
20
21 #include <Uefi.h>
22 #include <Library/DebugLib.h>
23 #include <Library/PrintLib.h>
24 #include <Library/UefiLib.h>
25 #include <Library/BaseMemoryLib.h>
26 #include <Library/DevicePathLib.h>
27 #include <Library/MemoryAllocationLib.h>
28 #include <Library/UefiBootServicesTableLib.h>
29 #include <Library/UefiRuntimeServicesTableLib.h>
30 #include <Library/UefiApplicationEntryPoint.h>
31 #include <Protocol/LoadedImage.h>
32 #include <Guid/FileInfo.h>
33 #include <Guid/FileSystemInfo.h>
34 #include <Protocol/BlockIo.h>
35 #include <Protocol/RamDisk.h>
36 #include <Protocol/SimpleFileSystem.h>
37 #include <Ventoy.h>
38
39 UINT8 *g_iso_data_buf = NULL;
40 UINTN g_iso_buf_size = 0;
41 BOOLEAN gMemdiskMode = FALSE;
42 BOOLEAN gSector512Mode = FALSE;
43
44 ventoy_sector_flag *g_sector_flag = NULL;
45 UINT32 g_sector_flag_num = 0;
46
47 EFI_FILE_OPEN g_original_fopen = NULL;
48 EFI_FILE_CLOSE g_original_fclose = NULL;
49 EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_OPEN_VOLUME g_original_open_volume = NULL;
50
51 /* EFI block device vendor device path GUID */
52 EFI_GUID gVtoyBlockDevicePathGuid = VTOY_BLOCK_DEVICE_PATH_GUID;
53
54 #define VENTOY_ISO9660_SECTOR_OVERFLOW 2097152
55
56 BOOLEAN g_fixup_iso9660_secover_enable = FALSE;
57 BOOLEAN g_fixup_iso9660_secover_start = FALSE;
58 UINT64 g_fixup_iso9660_secover_1st_secs = 0;
59 UINT64 g_fixup_iso9660_secover_cur_secs = 0;
60 UINT64 g_fixup_iso9660_secover_tot_secs = 0;
61
62 STATIC UINTN g_keyboard_hook_count = 0;
63 STATIC BOOLEAN g_blockio_start_record_bcd = FALSE;
64 STATIC BOOLEAN g_blockio_bcd_read_done = FALSE;
65
66 EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *g_con_simple_input_ex = NULL;
67 STATIC EFI_INPUT_READ_KEY_EX g_org_read_key_ex = NULL;
68 STATIC EFI_INPUT_READ_KEY g_org_read_key = NULL;
69
70 STATIC EFI_LOCATE_HANDLE g_org_locate_handle = NULL;
71
72 STATIC UINT8 g_sector_buf[2048];
73 STATIC EFI_BLOCK_READ g_sector_2048_read = NULL;
74 STATIC EFI_BLOCK_WRITE g_sector_2048_write = NULL;
75
76 BOOLEAN ventoy_is_cdrom_dp_exist(VOID)
77 {
78 UINTN i = 0;
79 UINTN Count = 0;
80 EFI_HANDLE *Handles = NULL;
81 EFI_STATUS Status = EFI_SUCCESS;
82 EFI_DEVICE_PATH_PROTOCOL *DevicePath = NULL;
83
84 Status = gBS->LocateHandleBuffer(ByProtocol, &gEfiDevicePathProtocolGuid,
85 NULL, &Count, &Handles);
86 if (EFI_ERROR(Status))
87 {
88 return FALSE;
89 }
90
91 for (i = 0; i < Count; i++)
92 {
93 Status = gBS->HandleProtocol(Handles[i], &gEfiDevicePathProtocolGuid, (VOID **)&DevicePath);
94 if (EFI_ERROR(Status))
95 {
96 continue;
97 }
98
99 while (!IsDevicePathEnd(DevicePath))
100 {
101 if (MEDIA_DEVICE_PATH == DevicePath->Type && MEDIA_CDROM_DP == DevicePath->SubType)
102 {
103 FreePool(Handles);
104 return TRUE;
105 }
106
107 DevicePath = NextDevicePathNode(DevicePath);
108 }
109 }
110
111 FreePool(Handles);
112 return FALSE;
113 }
114
115 #if 0
116 /* Block IO procotol */
117 #endif
118
119 EFI_STATUS EFIAPI ventoy_block_io_reset
120 (
121 IN EFI_BLOCK_IO_PROTOCOL *This,
122 IN BOOLEAN ExtendedVerification
123 )
124 {
125 (VOID)This;
126 (VOID)ExtendedVerification;
127 return EFI_SUCCESS;
128 }
129
130 STATIC EFI_STATUS EFIAPI ventoy_read_iso_sector
131 (
132 IN UINT64 Sector,
133 IN UINTN Count,
134 OUT VOID *Buffer
135 )
136 {
137 EFI_STATUS Status = EFI_SUCCESS;
138 EFI_LBA MapLba = 0;
139 UINT32 i = 0;
140 UINTN secLeft = 0;
141 UINTN secRead = 0;
142 UINT64 ReadStart = 0;
143 UINT64 ReadEnd = 0;
144 UINT64 OverrideStart = 0;
145 UINT64 OverrideEnd= 0;
146 UINT8 *pCurBuf = (UINT8 *)Buffer;
147 ventoy_img_chunk *pchunk = g_chunk;
148 ventoy_override_chunk *pOverride = g_override_chunk;
149 EFI_BLOCK_IO_PROTOCOL *pRawBlockIo = gBlockData.pRawBlockIo;
150
151 debug("read iso sector %lu count %u", Sector, Count);
152
153 ReadStart = Sector * 2048;
154 ReadEnd = (Sector + Count) * 2048;
155
156 for (i = 0; Count > 0 && i < g_img_chunk_num; i++, pchunk++)
157 {
158 if (Sector >= pchunk->img_start_sector && Sector <= pchunk->img_end_sector)
159 {
160 if (g_chain->disk_sector_size == 512)
161 {
162 MapLba = (Sector - pchunk->img_start_sector) * 4 + pchunk->disk_start_sector;
163 }
164 else if (g_chain->disk_sector_size == 1024)
165 {
166 MapLba = (Sector - pchunk->img_start_sector) * 2 + pchunk->disk_start_sector;
167 }
168 else if (g_chain->disk_sector_size == 2048)
169 {
170 MapLba = (Sector - pchunk->img_start_sector) + pchunk->disk_start_sector;
171 }
172 else if (g_chain->disk_sector_size == 4096)
173 {
174 MapLba = ((Sector - pchunk->img_start_sector) >> 1) + pchunk->disk_start_sector;
175 }
176
177
178 secLeft = pchunk->img_end_sector + 1 - Sector;
179 secRead = (Count < secLeft) ? Count : secLeft;
180
181 Status = pRawBlockIo->ReadBlocks(pRawBlockIo, pRawBlockIo->Media->MediaId,
182 MapLba, secRead * 2048, pCurBuf);
183 if (EFI_ERROR(Status))
184 {
185 debug("Raw disk read block failed %r LBA:%lu Count:%u", Status, MapLba, secRead);
186 return Status;
187 }
188
189 Count -= secRead;
190 Sector += secRead;
191 pCurBuf += secRead * 2048;
192 }
193 }
194
195 if (ReadStart > g_chain->real_img_size_in_bytes)
196 {
197 return EFI_SUCCESS;
198 }
199
200 /* override data */
201 pCurBuf = (UINT8 *)Buffer;
202 for (i = 0; i < g_override_chunk_num; i++, pOverride++)
203 {
204 OverrideStart = pOverride->img_offset;
205 OverrideEnd = pOverride->img_offset + pOverride->override_size;
206
207 if (OverrideStart >= ReadEnd || ReadStart >= OverrideEnd)
208 {
209 continue;
210 }
211
212 if (ReadStart <= OverrideStart)
213 {
214 if (ReadEnd <= OverrideEnd)
215 {
216 CopyMem(pCurBuf + OverrideStart - ReadStart, pOverride->override_data, ReadEnd - OverrideStart);
217 }
218 else
219 {
220 CopyMem(pCurBuf + OverrideStart - ReadStart, pOverride->override_data, pOverride->override_size);
221 }
222 }
223 else
224 {
225 if (ReadEnd <= OverrideEnd)
226 {
227 CopyMem(pCurBuf, pOverride->override_data + ReadStart - OverrideStart, ReadEnd - ReadStart);
228 }
229 else
230 {
231 CopyMem(pCurBuf, pOverride->override_data + ReadStart - OverrideStart, OverrideEnd - ReadStart);
232 }
233 }
234
235 if (g_fixup_iso9660_secover_enable && (!g_fixup_iso9660_secover_start) &&
236 pOverride->override_size == sizeof(ventoy_iso9660_override))
237 {
238 ventoy_iso9660_override *dirent = (ventoy_iso9660_override *)pOverride->override_data;
239 if (dirent->first_sector >= VENTOY_ISO9660_SECTOR_OVERFLOW)
240 {
241 g_fixup_iso9660_secover_start = TRUE;
242 g_fixup_iso9660_secover_cur_secs = 0;
243 }
244 }
245 }
246
247 if (g_blockio_start_record_bcd && FALSE == g_blockio_bcd_read_done)
248 {
249 if (*(UINT32 *)Buffer == 0x66676572)
250 {
251 g_blockio_bcd_read_done = TRUE;
252 }
253 }
254
255 return EFI_SUCCESS;
256 }
257
258 STATIC EFI_STATUS EFIAPI ventoy_write_iso_sector
259 (
260 IN UINT64 Sector,
261 IN UINTN Count,
262 IN VOID *Buffer
263 )
264 {
265 EFI_STATUS Status = EFI_SUCCESS;
266 EFI_LBA MapLba = 0;
267 UINT32 i = 0;
268 UINTN secLeft = 0;
269 UINTN secRead = 0;
270 UINT64 ReadStart = 0;
271 UINT64 ReadEnd = 0;
272 UINT8 *pCurBuf = (UINT8 *)Buffer;
273 ventoy_img_chunk *pchunk = g_chunk;
274 EFI_BLOCK_IO_PROTOCOL *pRawBlockIo = gBlockData.pRawBlockIo;
275
276 debug("write iso sector %lu count %u", Sector, Count);
277
278 ReadStart = Sector * 2048;
279 ReadEnd = (Sector + Count) * 2048;
280
281 for (i = 0; Count > 0 && i < g_img_chunk_num; i++, pchunk++)
282 {
283 if (Sector >= pchunk->img_start_sector && Sector <= pchunk->img_end_sector)
284 {
285 if (g_chain->disk_sector_size == 512)
286 {
287 MapLba = (Sector - pchunk->img_start_sector) * 4 + pchunk->disk_start_sector;
288 }
289 else if (g_chain->disk_sector_size == 1024)
290 {
291 MapLba = (Sector - pchunk->img_start_sector) * 2 + pchunk->disk_start_sector;
292 }
293 else if (g_chain->disk_sector_size == 2048)
294 {
295 MapLba = (Sector - pchunk->img_start_sector) + pchunk->disk_start_sector;
296 }
297 else if (g_chain->disk_sector_size == 4096)
298 {
299 MapLba = ((Sector - pchunk->img_start_sector) >> 1) + pchunk->disk_start_sector;
300 }
301
302
303 secLeft = pchunk->img_end_sector + 1 - Sector;
304 secRead = (Count < secLeft) ? Count : secLeft;
305
306 Status = pRawBlockIo->WriteBlocks(pRawBlockIo, pRawBlockIo->Media->MediaId,
307 MapLba, secRead * 2048, pCurBuf);
308 if (EFI_ERROR(Status))
309 {
310 debug("Raw disk write block failed %r LBA:%lu Count:%u", Status, MapLba, secRead);
311 return Status;
312 }
313
314 Count -= secRead;
315 Sector += secRead;
316 pCurBuf += secRead * 2048;
317 }
318 }
319
320 return EFI_SUCCESS;
321 }
322
323 EFI_STATUS EFIAPI ventoy_block_io_ramdisk_write
324 (
325 IN EFI_BLOCK_IO_PROTOCOL *This,
326 IN UINT32 MediaId,
327 IN EFI_LBA Lba,
328 IN UINTN BufferSize,
329 IN VOID *Buffer
330 )
331 {
332 (VOID)This;
333 (VOID)MediaId;
334 (VOID)Lba;
335 (VOID)BufferSize;
336 (VOID)Buffer;
337
338 if (!gSector512Mode)
339 {
340 return EFI_WRITE_PROTECTED;
341 }
342
343 CopyMem(g_iso_data_buf + (Lba * 2048), Buffer, BufferSize);
344
345 return EFI_SUCCESS;
346 }
347
348 EFI_STATUS EFIAPI ventoy_block_io_ramdisk_read
349 (
350 IN EFI_BLOCK_IO_PROTOCOL *This,
351 IN UINT32 MediaId,
352 IN EFI_LBA Lba,
353 IN UINTN BufferSize,
354 OUT VOID *Buffer
355 )
356 {
357 //debug("### ventoy_block_io_ramdisk_read sector:%u count:%u", (UINT32)Lba, (UINT32)BufferSize / 2048);
358
359 (VOID)This;
360 (VOID)MediaId;
361
362 CopyMem(Buffer, g_iso_data_buf + (Lba * 2048), BufferSize);
363
364 if (g_blockio_start_record_bcd && FALSE == g_blockio_bcd_read_done)
365 {
366 if (*(UINT32 *)Buffer == 0x66676572)
367 {
368 g_blockio_bcd_read_done = TRUE;
369 }
370 }
371
372 return EFI_SUCCESS;
373 }
374
375 EFI_LBA EFIAPI ventoy_fixup_iso9660_sector(IN EFI_LBA Lba, UINT32 secNum)
376 {
377 UINT32 i = 0;
378
379 if (g_fixup_iso9660_secover_cur_secs > 0)
380 {
381 Lba += VENTOY_ISO9660_SECTOR_OVERFLOW;
382 g_fixup_iso9660_secover_cur_secs += secNum;
383 if (g_fixup_iso9660_secover_cur_secs >= g_fixup_iso9660_secover_tot_secs)
384 {
385 g_fixup_iso9660_secover_start = FALSE;
386 goto end;
387 }
388 }
389 else
390 {
391 ventoy_iso9660_override *dirent;
392 ventoy_override_chunk *pOverride;
393
394 for (i = 0, pOverride = g_override_chunk; i < g_override_chunk_num; i++, pOverride++)
395 {
396 dirent = (ventoy_iso9660_override *)pOverride->override_data;
397 if (Lba == dirent->first_sector)
398 {
399 g_fixup_iso9660_secover_start = FALSE;
400 goto end;
401 }
402 }
403
404 if (g_fixup_iso9660_secover_start)
405 {
406 for (i = 0, pOverride = g_override_chunk; i < g_override_chunk_num; i++, pOverride++)
407 {
408 dirent = (ventoy_iso9660_override *)pOverride->override_data;
409 if (Lba + VENTOY_ISO9660_SECTOR_OVERFLOW == dirent->first_sector)
410 {
411 g_fixup_iso9660_secover_tot_secs = (dirent->size + 2047) / 2048;
412 g_fixup_iso9660_secover_cur_secs = secNum;
413 if (g_fixup_iso9660_secover_cur_secs >= g_fixup_iso9660_secover_tot_secs)
414 {
415 g_fixup_iso9660_secover_start = FALSE;
416 }
417 Lba += VENTOY_ISO9660_SECTOR_OVERFLOW;
418 goto end;
419 }
420 }
421 }
422 }
423
424 end:
425 return Lba;
426 }
427
428 EFI_STATUS EFIAPI ventoy_block_io_read
429 (
430 IN EFI_BLOCK_IO_PROTOCOL *This,
431 IN UINT32 MediaId,
432 IN EFI_LBA Lba,
433 IN UINTN BufferSize,
434 OUT VOID *Buffer
435 )
436 {
437 UINT32 i = 0;
438 UINT32 j = 0;
439 UINT32 lbacount = 0;
440 UINT32 secNum = 0;
441 UINT64 offset = 0;
442 EFI_LBA curlba = 0;
443 EFI_LBA lastlba = 0;
444 UINT8 *lastbuffer;
445 ventoy_sector_flag *cur_flag;
446 ventoy_virt_chunk *node;
447
448 //debug("### ventoy_block_io_read sector:%u count:%u", (UINT32)Lba, (UINT32)BufferSize / 2048);
449
450 secNum = BufferSize / 2048;
451
452 /* Workaround for SSTR PE loader error */
453 if (g_fixup_iso9660_secover_start)
454 {
455 Lba = ventoy_fixup_iso9660_sector(Lba, secNum);
456 }
457
458 offset = Lba * 2048;
459
460 if (offset + BufferSize <= g_chain->real_img_size_in_bytes)
461 {
462 return ventoy_read_iso_sector(Lba, secNum, Buffer);
463 }
464
465 if (secNum > g_sector_flag_num)
466 {
467 cur_flag = AllocatePool(secNum * sizeof(ventoy_sector_flag));
468 if (NULL == cur_flag)
469 {
470 return EFI_OUT_OF_RESOURCES;
471 }
472
473 FreePool(g_sector_flag);
474 g_sector_flag = cur_flag;
475 g_sector_flag_num = secNum;
476 }
477
478 for (curlba = Lba, cur_flag = g_sector_flag, j = 0; j < secNum; j++, curlba++, cur_flag++)
479 {
480 cur_flag->flag = 0;
481 for (node = g_virt_chunk, i = 0; i < g_virt_chunk_num; i++, node++)
482 {
483 if (curlba >= node->mem_sector_start && curlba < node->mem_sector_end)
484 {
485 CopyMem((UINT8 *)Buffer + j * 2048,
486 (char *)g_virt_chunk + node->mem_sector_offset + (curlba - node->mem_sector_start) * 2048,
487 2048);
488 cur_flag->flag = 1;
489 break;
490 }
491 else if (curlba >= node->remap_sector_start && curlba < node->remap_sector_end)
492 {
493 cur_flag->remap_lba = node->org_sector_start + curlba - node->remap_sector_start;
494 cur_flag->flag = 2;
495 break;
496 }
497 }
498 }
499
500 for (curlba = Lba, cur_flag = g_sector_flag, j = 0; j < secNum; j++, curlba++, cur_flag++)
501 {
502 if (cur_flag->flag == 2)
503 {
504 if (lastlba == 0)
505 {
506 lastbuffer = (UINT8 *)Buffer + j * 2048;
507 lastlba = cur_flag->remap_lba;
508 lbacount = 1;
509 }
510 else if (lastlba + lbacount == cur_flag->remap_lba)
511 {
512 lbacount++;
513 }
514 else
515 {
516 ventoy_read_iso_sector(lastlba, lbacount, lastbuffer);
517 lastbuffer = (UINT8 *)Buffer + j * 2048;
518 lastlba = cur_flag->remap_lba;
519 lbacount = 1;
520 }
521 }
522 }
523
524 if (lbacount > 0)
525 {
526 ventoy_read_iso_sector(lastlba, lbacount, lastbuffer);
527 }
528
529 return EFI_SUCCESS;
530 }
531
532 EFI_STATUS EFIAPI ventoy_block_io_write
533 (
534 IN EFI_BLOCK_IO_PROTOCOL *This,
535 IN UINT32 MediaId,
536 IN EFI_LBA Lba,
537 IN UINTN BufferSize,
538 IN VOID *Buffer
539 )
540 {
541 UINT32 secNum = 0;
542 UINT64 offset = 0;
543
544 (VOID)This;
545 (VOID)MediaId;
546
547 if (!gSector512Mode)
548 {
549 return EFI_WRITE_PROTECTED;
550 }
551
552 secNum = BufferSize / 2048;
553 offset = Lba * 2048;
554
555 return ventoy_write_iso_sector(Lba, secNum, Buffer);
556 }
557
558 EFI_STATUS EFIAPI ventoy_block_io_flush(IN EFI_BLOCK_IO_PROTOCOL *This)
559 {
560 (VOID)This;
561 return EFI_SUCCESS;
562 }
563
564
565 EFI_STATUS EFIAPI ventoy_fill_device_path(VOID)
566 {
567 UINTN NameLen = 0;
568 UINT8 TmpBuf[128] = {0};
569 VENDOR_DEVICE_PATH *venPath = NULL;
570
571 venPath = (VENDOR_DEVICE_PATH *)TmpBuf;
572 NameLen = StrSize(VTOY_BLOCK_DEVICE_PATH_NAME);
573 venPath->Header.Type = HARDWARE_DEVICE_PATH;
574 venPath->Header.SubType = HW_VENDOR_DP;
575 venPath->Header.Length[0] = sizeof(VENDOR_DEVICE_PATH) + NameLen;
576 venPath->Header.Length[1] = 0;
577 CopyMem(&venPath->Guid, &gVtoyBlockDevicePathGuid, sizeof(EFI_GUID));
578 CopyMem(venPath + 1, VTOY_BLOCK_DEVICE_PATH_NAME, NameLen);
579
580 gBlockData.Path = AppendDevicePathNode(NULL, (EFI_DEVICE_PATH_PROTOCOL *)TmpBuf);
581 gBlockData.DevicePathCompareLen = sizeof(VENDOR_DEVICE_PATH) + NameLen;
582
583 debug("gBlockData.Path=<%s>\n", ConvertDevicePathToText(gBlockData.Path, FALSE, FALSE));
584
585 return EFI_SUCCESS;
586 }
587
588 EFI_STATUS EFIAPI ventoy_connect_driver(IN EFI_HANDLE ControllerHandle, IN CONST CHAR16 *DrvName)
589 {
590 UINTN i = 0;
591 UINTN Count = 0;
592 CHAR16 *DriverName = NULL;
593 EFI_HANDLE *Handles = NULL;
594 EFI_HANDLE DrvHandles[2] = { NULL };
595 EFI_STATUS Status = EFI_SUCCESS;
596 EFI_COMPONENT_NAME_PROTOCOL *NameProtocol = NULL;
597 EFI_COMPONENT_NAME2_PROTOCOL *Name2Protocol = NULL;
598
599 debug("ventoy_connect_driver <%s>...", DrvName);
600
601 Status = gBS->LocateHandleBuffer(ByProtocol, &gEfiComponentName2ProtocolGuid,
602 NULL, &Count, &Handles);
603 if (EFI_ERROR(Status))
604 {
605 return Status;
606 }
607
608 for (i = 0; i < Count; i++)
609 {
610 Status = gBS->HandleProtocol(Handles[i], &gEfiComponentName2ProtocolGuid, (VOID **)&Name2Protocol);
611 if (EFI_ERROR(Status))
612 {
613 continue;
614 }
615
616 Status = Name2Protocol->GetDriverName(Name2Protocol, "en", &DriverName);
617 if (EFI_ERROR(Status) || NULL == DriverName)
618 {
619 continue;
620 }
621
622 if (StrStr(DriverName, DrvName))
623 {
624 debug("Find driver name2:<%s>: <%s>", DriverName, DrvName);
625 DrvHandles[0] = Handles[i];
626 break;
627 }
628 }
629
630 if (i < Count)
631 {
632 Status = gBS->ConnectController(ControllerHandle, DrvHandles, NULL, TRUE);
633 debug("ventoy_connect_driver:<%s> <%r>", DrvName, Status);
634 goto end;
635 }
636
637 debug("%s NOT found, now try COMPONENT_NAME", DrvName);
638
639 Count = 0;
640 FreePool(Handles);
641 Handles = NULL;
642
643 Status = gBS->LocateHandleBuffer(ByProtocol, &gEfiComponentNameProtocolGuid,
644 NULL, &Count, &Handles);
645 if (EFI_ERROR(Status))
646 {
647 return Status;
648 }
649
650 for (i = 0; i < Count; i++)
651 {
652 Status = gBS->HandleProtocol(Handles[i], &gEfiComponentNameProtocolGuid, (VOID **)&NameProtocol);
653 if (EFI_ERROR(Status))
654 {
655 continue;
656 }
657
658 Status = NameProtocol->GetDriverName(NameProtocol, "en", &DriverName);
659 if (EFI_ERROR(Status))
660 {
661 continue;
662 }
663
664 if (StrStr(DriverName, DrvName))
665 {
666 debug("Find driver name:<%s>: <%s>", DriverName, DrvName);
667 DrvHandles[0] = Handles[i];
668 break;
669 }
670 }
671
672 if (i < Count)
673 {
674 Status = gBS->ConnectController(ControllerHandle, DrvHandles, NULL, TRUE);
675 debug("ventoy_connect_driver:<%s> <%r>", DrvName, Status);
676 goto end;
677 }
678
679 Status = EFI_NOT_FOUND;
680
681 end:
682 FreePool(Handles);
683
684 return Status;
685 }
686
687 EFI_STATUS EFIAPI ventoy_block_io_read_512
688 (
689 IN EFI_BLOCK_IO_PROTOCOL *This,
690 IN UINT32 MediaId,
691 IN EFI_LBA Lba,
692 IN UINTN BufferSize,
693 OUT VOID *Buffer
694 )
695 {
696 EFI_LBA Mod;
697 UINTN ReadSize;
698 UINT8 *CurBuf = NULL;
699 EFI_STATUS Status = EFI_SUCCESS;
700
701 debug("ventoy_block_io_read_512 %lu %lu\n", Lba, BufferSize / 512);
702
703 CurBuf = (UINT8 *)Buffer;
704
705 Mod = Lba % 4;
706 if (Mod > 0)
707 {
708 Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
709
710 if (BufferSize <= (4 - Mod) * 512)
711 {
712 CopyMem(CurBuf, g_sector_buf + Mod * 512, BufferSize);
713 return EFI_SUCCESS;
714 }
715 else
716 {
717 ReadSize = (4 - Mod) * 512;
718 CopyMem(CurBuf, g_sector_buf + Mod * 512, ReadSize);
719 CurBuf += ReadSize;
720 Lba += (4 - Mod);
721 BufferSize -= ReadSize;
722 }
723 }
724
725 if (BufferSize >= 2048)
726 {
727 ReadSize = BufferSize / 2048 * 2048;
728
729 Status |= g_sector_2048_read(This, MediaId, Lba / 4, ReadSize, CurBuf);
730 CurBuf += ReadSize;
731
732 Lba += ReadSize / 512;
733 BufferSize -= ReadSize;
734 }
735
736 if (BufferSize > 0)
737 {
738 Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
739 CopyMem(CurBuf, g_sector_buf, BufferSize);
740 }
741
742 return Status;
743 }
744
745 EFI_STATUS EFIAPI ventoy_block_io_write_512
746 (
747 IN EFI_BLOCK_IO_PROTOCOL *This,
748 IN UINT32 MediaId,
749 IN EFI_LBA Lba,
750 IN UINTN BufferSize,
751 IN VOID *Buffer
752 )
753 {
754 EFI_LBA Mod;
755 UINTN ReadSize;
756 UINT8 *CurBuf = NULL;
757 EFI_STATUS Status = EFI_SUCCESS;
758
759 debug("ventoy_block_io_write_512 %lu %lu\n", Lba, BufferSize / 512);
760
761 CurBuf = (UINT8 *)Buffer;
762
763 Mod = Lba % 4;
764 if (Mod > 0)
765 {
766 Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
767
768 if (BufferSize <= (4 - Mod) * 512)
769 {
770 CopyMem(g_sector_buf + Mod * 512, CurBuf, BufferSize);
771 return g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
772 }
773 else
774 {
775 ReadSize = (4 - Mod) * 512;
776 CopyMem(g_sector_buf + Mod * 512, CurBuf, ReadSize);
777 g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
778
779 CurBuf += ReadSize;
780 Lba += (4 - Mod);
781 BufferSize -= ReadSize;
782 }
783 }
784
785 if (BufferSize >= 2048)
786 {
787 ReadSize = BufferSize / 2048 * 2048;
788
789 Status |= g_sector_2048_write(This, MediaId, Lba / 4, ReadSize, CurBuf);
790 CurBuf += ReadSize;
791
792 Lba += ReadSize / 512;
793 BufferSize -= ReadSize;
794 }
795
796 if (BufferSize > 0)
797 {
798 Status |= g_sector_2048_read(This, MediaId, Lba / 4, 2048, g_sector_buf);
799
800 CopyMem(g_sector_buf, CurBuf, BufferSize);
801 g_sector_2048_write(This, MediaId, Lba / 4, 2048, g_sector_buf);
802 }
803
804 return Status;
805 }
806
807 EFI_STATUS EFIAPI ventoy_install_blockio(IN EFI_HANDLE ImageHandle, IN UINT64 ImgSize)
808 {
809 EFI_STATUS Status = EFI_SUCCESS;
810 EFI_BLOCK_IO_PROTOCOL *pBlockIo = &(gBlockData.BlockIo);
811
812 ventoy_fill_device_path();
813
814 debug("install block io protocol %p", ImageHandle);
815 ventoy_debug_pause();
816
817 if (gSector512Mode)
818 {
819 gBlockData.Media.BlockSize = 512;
820 gBlockData.Media.LastBlock = ImgSize / 512 - 1;
821 }
822 else
823 {
824 gBlockData.Media.BlockSize = 2048;
825 gBlockData.Media.LastBlock = ImgSize / 2048 - 1;
826 }
827
828 gBlockData.Media.ReadOnly = TRUE;
829 gBlockData.Media.MediaPresent = 1;
830 gBlockData.Media.LogicalBlocksPerPhysicalBlock = 1;
831
832 pBlockIo->Revision = EFI_BLOCK_IO_PROTOCOL_REVISION3;
833 pBlockIo->Media = &(gBlockData.Media);
834 pBlockIo->Reset = ventoy_block_io_reset;
835
836 if (gSector512Mode)
837 {
838 g_sector_2048_read = gMemdiskMode ? ventoy_block_io_ramdisk_read : ventoy_block_io_read;
839 g_sector_2048_write = gMemdiskMode ? ventoy_block_io_ramdisk_write : ventoy_block_io_write;
840 pBlockIo->ReadBlocks = ventoy_block_io_read_512;
841 pBlockIo->WriteBlocks = ventoy_block_io_write_512;
842 }
843 else
844 {
845 pBlockIo->ReadBlocks = gMemdiskMode ? ventoy_block_io_ramdisk_read : ventoy_block_io_read;
846 pBlockIo->WriteBlocks = ventoy_block_io_write;
847 }
848
849 pBlockIo->FlushBlocks = ventoy_block_io_flush;
850
851 Status = gBS->InstallMultipleProtocolInterfaces(&gBlockData.Handle,
852 &gEfiBlockIoProtocolGuid, &gBlockData.BlockIo,
853 &gEfiDevicePathProtocolGuid, gBlockData.Path,
854 NULL);
855 debug("Install protocol %r %p", Status, gBlockData.Handle);
856 if (EFI_ERROR(Status))
857 {
858 return Status;
859 }
860
861 Status = ventoy_connect_driver(gBlockData.Handle, L"Disk I/O Driver");
862 debug("Connect disk IO driver %r", Status);
863
864 Status = ventoy_connect_driver(gBlockData.Handle, L"Partition Driver");
865 debug("Connect partition driver %r", Status);
866 if (EFI_ERROR(Status))
867 {
868 Status = gBS->ConnectController(gBlockData.Handle, NULL, NULL, TRUE);
869 debug("Connect all controller %r", Status);
870 }
871
872 ventoy_debug_pause();
873
874 return EFI_SUCCESS;
875 }
876
877 #if 0
878 /* For file replace */
879 #endif
880
881 STATIC EFI_STATUS EFIAPI
882 ventoy_wrapper_fs_open(EFI_FILE_HANDLE This, EFI_FILE_HANDLE *New, CHAR16 *Name, UINT64 Mode, UINT64 Attributes)
883 {
884 (VOID)This;
885 (VOID)New;
886 (VOID)Name;
887 (VOID)Mode;
888 (VOID)Attributes;
889 return EFI_SUCCESS;
890 }
891
892 STATIC EFI_STATUS EFIAPI
893 ventoy_wrapper_file_open_ex(EFI_FILE_HANDLE This, EFI_FILE_HANDLE *New, CHAR16 *Name, UINT64 Mode, UINT64 Attributes, EFI_FILE_IO_TOKEN *Token)
894 {
895 return ventoy_wrapper_fs_open(This, New, Name, Mode, Attributes);
896 }
897
898 STATIC EFI_STATUS EFIAPI
899 ventoy_wrapper_file_delete(EFI_FILE_HANDLE This)
900 {
901 (VOID)This;
902 return EFI_SUCCESS;
903 }
904
905 STATIC EFI_STATUS EFIAPI
906 ventoy_wrapper_file_set_info(EFI_FILE_HANDLE This, EFI_GUID *Type, UINTN Len, VOID *Data)
907 {
908 return EFI_SUCCESS;
909 }
910
911 STATIC EFI_STATUS EFIAPI
912 ventoy_wrapper_file_flush(EFI_FILE_HANDLE This)
913 {
914 (VOID)This;
915 return EFI_SUCCESS;
916 }
917
918 /* Ex version */
919 STATIC EFI_STATUS EFIAPI
920 ventoy_wrapper_file_flush_ex(EFI_FILE_HANDLE This, EFI_FILE_IO_TOKEN *Token)
921 {
922 (VOID)This;
923 (VOID)Token;
924 return EFI_SUCCESS;
925 }
926
927
928 STATIC EFI_STATUS EFIAPI
929 ventoy_wrapper_file_write(EFI_FILE_HANDLE This, UINTN *Len, VOID *Data)
930 {
931 (VOID)This;
932 (VOID)Len;
933 (VOID)Data;
934
935 return EFI_WRITE_PROTECTED;
936 }
937
938 STATIC EFI_STATUS EFIAPI
939 ventoy_wrapper_file_write_ex(IN EFI_FILE_PROTOCOL *This, IN OUT EFI_FILE_IO_TOKEN *Token)
940 {
941 return ventoy_wrapper_file_write(This, &(Token->BufferSize), Token->Buffer);
942 }
943
944
945 STATIC EFI_STATUS EFIAPI
946 ventoy_wrapper_file_close(EFI_FILE_HANDLE This)
947 {
948 (VOID)This;
949 return EFI_SUCCESS;
950 }
951
952
953 STATIC EFI_STATUS EFIAPI
954 ventoy_wrapper_file_set_pos(EFI_FILE_HANDLE This, UINT64 Position)
955 {
956 (VOID)This;
957
958 if (Position <= g_efi_file_replace.FileSizeBytes)
959 {
960 g_efi_file_replace.CurPos = Position;
961 }
962 else
963 {
964 g_efi_file_replace.CurPos = g_efi_file_replace.FileSizeBytes;
965 }
966
967 return EFI_SUCCESS;
968 }
969
970 STATIC EFI_STATUS EFIAPI
971 ventoy_wrapper_file_get_pos(EFI_FILE_HANDLE This, UINT64 *Position)
972 {
973 (VOID)This;
974
975 *Position = g_efi_file_replace.CurPos;
976
977 return EFI_SUCCESS;
978 }
979
980
981 STATIC EFI_STATUS EFIAPI
982 ventoy_wrapper_file_get_info(EFI_FILE_HANDLE This, EFI_GUID *Type, UINTN *Len, VOID *Data)
983 {
984 EFI_FILE_INFO *Info = (EFI_FILE_INFO *) Data;
985
986 debug("ventoy_wrapper_file_get_info ... %u", *Len);
987
988 if (!CompareGuid(Type, &gEfiFileInfoGuid))
989 {
990 return EFI_INVALID_PARAMETER;
991 }
992
993 if (*Len == 0)
994 {
995 *Len = 384;
996 return EFI_BUFFER_TOO_SMALL;
997 }
998
999 ZeroMem(Data, sizeof(EFI_FILE_INFO));
1000
1001 Info->Size = sizeof(EFI_FILE_INFO);
1002 Info->FileSize = g_efi_file_replace.FileSizeBytes;
1003 Info->PhysicalSize = g_efi_file_replace.FileSizeBytes;
1004 Info->Attribute = EFI_FILE_READ_ONLY;
1005 //Info->FileName = EFI_FILE_READ_ONLY;
1006
1007 *Len = Info->Size;
1008
1009 return EFI_SUCCESS;
1010 }
1011
1012 STATIC EFI_STATUS EFIAPI
1013 ventoy_wrapper_file_read(EFI_FILE_HANDLE This, UINTN *Len, VOID *Data)
1014 {
1015 EFI_LBA Lba;
1016 UINTN ReadLen = *Len;
1017
1018 (VOID)This;
1019
1020 debug("ventoy_wrapper_file_read ... %u", *Len);
1021
1022 if (g_efi_file_replace.CurPos + ReadLen > g_efi_file_replace.FileSizeBytes)
1023 {
1024 ReadLen = g_efi_file_replace.FileSizeBytes - g_efi_file_replace.CurPos;
1025 }
1026
1027 Lba = g_efi_file_replace.CurPos / 2048 + g_efi_file_replace.BlockIoSectorStart;
1028
1029 ventoy_block_io_read(NULL, 0, Lba, ReadLen, Data);
1030
1031 *Len = ReadLen;
1032
1033 g_efi_file_replace.CurPos += ReadLen;
1034
1035 return EFI_SUCCESS;
1036 }
1037
1038 STATIC EFI_STATUS EFIAPI
1039 ventoy_wrapper_file_read_ex(IN EFI_FILE_PROTOCOL *This, IN OUT EFI_FILE_IO_TOKEN *Token)
1040 {
1041 return ventoy_wrapper_file_read(This, &(Token->BufferSize), Token->Buffer);
1042 }
1043
1044 STATIC EFI_STATUS EFIAPI ventoy_wrapper_file_procotol(EFI_FILE_PROTOCOL *File)
1045 {
1046 File->Revision = EFI_FILE_PROTOCOL_REVISION2;
1047 File->Open = ventoy_wrapper_fs_open;
1048 File->Close = ventoy_wrapper_file_close;
1049 File->Delete = ventoy_wrapper_file_delete;
1050 File->Read = ventoy_wrapper_file_read;
1051 File->Write = ventoy_wrapper_file_write;
1052 File->GetPosition = ventoy_wrapper_file_get_pos;
1053 File->SetPosition = ventoy_wrapper_file_set_pos;
1054 File->GetInfo = ventoy_wrapper_file_get_info;
1055 File->SetInfo = ventoy_wrapper_file_set_info;
1056 File->Flush = ventoy_wrapper_file_flush;
1057 File->OpenEx = ventoy_wrapper_file_open_ex;
1058 File->ReadEx = ventoy_wrapper_file_read_ex;
1059 File->WriteEx = ventoy_wrapper_file_write_ex;
1060 File->FlushEx = ventoy_wrapper_file_flush_ex;
1061
1062 return EFI_SUCCESS;
1063 }
1064
1065 STATIC EFI_STATUS EFIAPI ventoy_wrapper_file_open
1066 (
1067 EFI_FILE_HANDLE This,
1068 EFI_FILE_HANDLE *New,
1069 CHAR16 *Name,
1070 UINT64 Mode,
1071 UINT64 Attributes
1072 )
1073 {
1074 UINT32 i = 0;
1075 UINT32 j = 0;
1076 UINT64 Sectors = 0;
1077 EFI_STATUS Status = EFI_SUCCESS;
1078 CHAR8 TmpName[256];
1079 ventoy_virt_chunk *virt = NULL;
1080
1081 debug("## ventoy_wrapper_file_open <%s> ", Name);
1082
1083 Status = g_original_fopen(This, New, Name, Mode, Attributes);
1084 if (EFI_ERROR(Status))
1085 {
1086 return Status;
1087 }
1088
1089 if (g_file_replace_list && g_file_replace_list->magic == GRUB_FILE_REPLACE_MAGIC &&
1090 g_file_replace_list->new_file_virtual_id < g_virt_chunk_num)
1091 {
1092 AsciiSPrint(TmpName, sizeof(TmpName), "%s", Name);
1093 for (j = 0; j < 4; j++)
1094 {
1095 if (0 == AsciiStrCmp(g_file_replace_list[i].old_file_name[j], TmpName))
1096 {
1097 g_original_fclose(*New);
1098 *New = &g_efi_file_replace.WrapperHandle;
1099 ventoy_wrapper_file_procotol(*New);
1100
1101 virt = g_virt_chunk + g_file_replace_list->new_file_virtual_id;
1102
1103 Sectors = (virt->mem_sector_end - virt->mem_sector_start) + (virt->remap_sector_end - virt->remap_sector_start);
1104
1105 g_efi_file_replace.BlockIoSectorStart = virt->mem_sector_start;
1106 g_efi_file_replace.FileSizeBytes = Sectors * 2048;
1107
1108 if (gDebugPrint)
1109 {
1110 debug("## ventoy_wrapper_file_open <%s> BlockStart:%lu Sectors:%lu Bytes:%lu", Name,
1111 g_efi_file_replace.BlockIoSectorStart, Sectors, Sectors * 2048);
1112 sleep(3);
1113 }
1114
1115 return Status;
1116 }
1117 }
1118
1119 if (StrCmp(Name, L"\\EFI\\BOOT") == 0)
1120 {
1121 (*New)->Open = ventoy_wrapper_file_open;
1122 }
1123 }
1124
1125 return Status;
1126 }
1127
1128 EFI_STATUS EFIAPI ventoy_wrapper_open_volume
1129 (
1130 IN EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *This,
1131 OUT EFI_FILE_PROTOCOL **Root
1132 )
1133 {
1134 EFI_STATUS Status = EFI_SUCCESS;
1135
1136 Status = g_original_open_volume(This, Root);
1137 if (!EFI_ERROR(Status))
1138 {
1139 g_original_fopen = (*Root)->Open;
1140 g_original_fclose = (*Root)->Close;
1141 (*Root)->Open = ventoy_wrapper_file_open;
1142 }
1143
1144 return Status;
1145 }
1146
1147 EFI_STATUS EFIAPI ventoy_wrapper_push_openvolume(IN EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_OPEN_VOLUME OpenVolume)
1148 {
1149 g_original_open_volume = OpenVolume;
1150 return EFI_SUCCESS;
1151 }
1152
1153 #if 0
1154 /* For auto skip Windows 'Press any key to boot from CD or DVD ...' */
1155 #endif
1156
1157 STATIC EFI_STATUS EFIAPI ventoy_wrapper_read_key_ex
1158 (
1159 IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
1160 OUT EFI_KEY_DATA *KeyData
1161 )
1162 {
1163 /* only hook once before BCD file read */
1164 if (g_keyboard_hook_count == 0 && g_blockio_bcd_read_done == FALSE)
1165 {
1166 g_keyboard_hook_count++;
1167
1168 KeyData->Key.ScanCode = SCAN_DELETE;
1169 KeyData->Key.UnicodeChar = 0;
1170 KeyData->KeyState.KeyShiftState = 0;
1171 KeyData->KeyState.KeyToggleState = 0;
1172
1173 return EFI_SUCCESS;
1174 }
1175
1176 return g_org_read_key_ex(This, KeyData);
1177 }
1178
1179 EFI_STATUS EFIAPI ventoy_wrapper_read_key
1180 (
1181 IN EFI_SIMPLE_TEXT_INPUT_PROTOCOL *This,
1182 OUT EFI_INPUT_KEY *Key
1183 )
1184 {
1185 /* only hook once before BCD file read */
1186 if (g_keyboard_hook_count == 0 && g_blockio_bcd_read_done == FALSE)
1187 {
1188 g_keyboard_hook_count++;
1189
1190 Key->ScanCode = SCAN_DELETE;
1191 Key->UnicodeChar = 0;
1192 return EFI_SUCCESS;
1193 }
1194
1195 return g_org_read_key(This, Key);
1196 }
1197
1198 EFI_STATUS ventoy_hook_keyboard_start(VOID)
1199 {
1200 g_blockio_start_record_bcd = TRUE;
1201 g_blockio_bcd_read_done = FALSE;
1202 g_keyboard_hook_count = 0;
1203
1204 if (g_con_simple_input_ex)
1205 {
1206 g_org_read_key_ex = g_con_simple_input_ex->ReadKeyStrokeEx;
1207 g_con_simple_input_ex->ReadKeyStrokeEx = ventoy_wrapper_read_key_ex;
1208 }
1209
1210 g_org_read_key = gST->ConIn->ReadKeyStroke;
1211 gST->ConIn->ReadKeyStroke = ventoy_wrapper_read_key;
1212
1213 return EFI_SUCCESS;
1214 }
1215
1216 EFI_STATUS ventoy_hook_keyboard_stop(VOID)
1217 {
1218 g_blockio_start_record_bcd = FALSE;
1219 g_blockio_bcd_read_done = FALSE;
1220 g_keyboard_hook_count = 0;
1221
1222 if (g_con_simple_input_ex)
1223 {
1224 g_con_simple_input_ex->ReadKeyStrokeEx = g_org_read_key_ex;
1225 }
1226
1227 gST->ConIn->ReadKeyStroke = g_org_read_key;
1228
1229 return EFI_SUCCESS;
1230 }
1231
1232 #if 0
1233 /* Fixup the 1st cdrom influnce for Windows boot */
1234 #endif
1235
1236 STATIC EFI_STATUS EFIAPI ventoy_wrapper_locate_handle
1237 (
1238 IN EFI_LOCATE_SEARCH_TYPE SearchType,
1239 IN EFI_GUID *Protocol, OPTIONAL
1240 IN VOID *SearchKey, OPTIONAL
1241 IN OUT UINTN *BufferSize,
1242 OUT EFI_HANDLE *Buffer
1243 )
1244 {
1245 UINTN i;
1246 EFI_HANDLE Handle = NULL;
1247 EFI_STATUS Status = EFI_SUCCESS;
1248
1249 Status = g_org_locate_handle(SearchType, Protocol, SearchKey, BufferSize, Buffer);
1250
1251 if (EFI_SUCCESS == Status && Protocol && CompareGuid(&gEfiBlockIoProtocolGuid, Protocol))
1252 {
1253 for (i = 0; i < (*BufferSize) / sizeof(EFI_HANDLE); i++)
1254 {
1255 if (Buffer[i] == gBlockData.Handle)
1256 {
1257 Handle = Buffer[0];
1258 Buffer[0] = Buffer[i];
1259 Buffer[i] = Handle;
1260 break;
1261 }
1262 }
1263 }
1264
1265 return Status;
1266 }
1267
1268 EFI_STATUS ventoy_hook_1st_cdrom_start(VOID)
1269 {
1270 g_org_locate_handle = gBS->LocateHandle;
1271 gBS->LocateHandle = ventoy_wrapper_locate_handle;
1272
1273 return EFI_SUCCESS;
1274 }
1275
1276 EFI_STATUS ventoy_hook_1st_cdrom_stop(VOID)
1277 {
1278 gBS->LocateHandle = g_org_locate_handle;
1279 g_org_locate_handle = NULL;
1280
1281 return EFI_SUCCESS;
1282 }
1283
https://github.com/ventoy/Ventoy