IPXE/ipxe-3fe683e/src/arch/x86/core/ventoy_vdisk.c

   1
   2 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
   3
   4 #include <stdint.h>
   5 #include <stdlib.h>
   6 #include <stdio.h>
   7 #include <limits.h>
   8 #include <byteswap.h>
   9 #include <errno.h>
  10 #include <assert.h>
  11 #include <ipxe/blockdev.h>
  12 #include <ipxe/io.h>
  13 #include <ipxe/acpi.h>
  14 #include <ipxe/sanboot.h>
  15 #include <ipxe/device.h>
  16 #include <ipxe/pci.h>
  17 #include <ipxe/eltorito.h>
  18 #include <ipxe/timer.h>
  19 #include <ipxe/umalloc.h>
  20 #include <realmode.h>
  21 #include <bios.h>
  22 #include <biosint.h>
  23 #include <bootsector.h>
  24 #include <int13.h>
  25 #include <ventoy.h>
  26
  27 int g_debug = 0;
  28 char *g_cmdline_copy;
  29 void *g_initrd_addr;
  30 size_t g_initrd_len;
  31 ventoy_chain_head *g_chain;
  32 ventoy_img_chunk *g_chunk;
  33 uint32_t g_img_chunk_num;
  34 ventoy_img_chunk *g_cur_chunk;
  35 uint32_t g_disk_sector_size;
  36
  37 ventoy_override_chunk *g_override_chunk;
  38 uint32_t g_override_chunk_num;
  39
  40 ventoy_virt_chunk *g_virt_chunk;
  41 uint32_t g_virt_chunk_num;
  42
  43 ventoy_sector_flag g_sector_flag[128];
  44
  45 static struct int13_disk_address __bss16 ( ventoy_address );
  46 #define ventoy_address __use_data16 ( ventoy_address )
  47
  48 static uint64_t ventoy_remap_lba(uint64_t lba, uint32_t *count)
  49 {
  50     uint32_t i;
  51     uint32_t max_sectors;
  52     ventoy_img_chunk *cur;
  53
  54     if ((NULL == g_cur_chunk) || ((lba) < g_cur_chunk->img_start_sector) || ((lba) > g_cur_chunk->img_end_sector))
  55     {
  56         g_cur_chunk = NULL;
  57         for (i = 0; i < g_img_chunk_num; i++)
  58         {
  59             cur = g_chunk + i;
  60             if (lba >= cur->img_start_sector && lba <= cur->img_end_sector)
  61             {
  62                 g_cur_chunk = cur;
  63                 break;
  64             }
  65         }
  66     }
  67
  68     if (g_cur_chunk)
  69     {
  70         max_sectors = g_cur_chunk->img_end_sector - lba + 1;
  71         if (*count > max_sectors)
  72         {
  73             *count = max_sectors;
  74         }
  75
  76         if (512 == g_disk_sector_size)
  77         {
  78             return g_cur_chunk->disk_start_sector + ((lba - g_cur_chunk->img_start_sector) << 2);
  79         }
  80         return g_cur_chunk->disk_start_sector + (lba - g_cur_chunk->img_start_sector) * 2048 / g_disk_sector_size;
  81     }
  82     return lba;
  83 }
  84
  85 static int ventoy_vdisk_read_real(uint64_t lba, unsigned int count, unsigned long buffer)
  86 {
  87     uint32_t i = 0;
  88     uint32_t left = 0;
  89     uint32_t readcount = 0;
  90     uint32_t tmpcount = 0;
  91     uint16_t status = 0;
  92     uint64_t curlba = 0;
  93     uint64_t maplba = 0;
  94     uint64_t start = 0;
  95     uint64_t end = 0;
  96     uint64_t override_start = 0;
  97     uint64_t override_end = 0;
  98     unsigned long phyaddr;
  99     unsigned long databuffer = buffer;
 100     uint8_t *override_data;
 101
 102     curlba = lba;
 103     left = count;
 104
 105     while (left > 0)
 106     {
 107         readcount = left;
 108         maplba = ventoy_remap_lba(curlba, &readcount);
 109
 110         if (g_disk_sector_size == 512)
 111         {
 112             tmpcount = (readcount << 2);
 113         }
 114         else
 115         {
 116             tmpcount = (readcount * 2048) / g_disk_sector_size;
 117         }
 118
 119         phyaddr = user_to_phys(buffer, 0);
 120
 121         while (tmpcount > 0)
 122         {
 123             /* Use INT 13, 42 to read the data from real disk */
 124             ventoy_address.lba = maplba;
 125             ventoy_address.buffer.segment = (uint16_t)(phyaddr >> 4);
 126             ventoy_address.buffer.offset = (uint16_t)(phyaddr & 0x0F);
 127
 128             if (tmpcount >= 64) /* max sectors per transmit */
 129             {
 130                 ventoy_address.count = 64;
 131                 tmpcount -= 64;
 132                 maplba   += 64;
 133                 phyaddr  += 32768;
 134             }
 135             else
 136             {
 137                 ventoy_address.count = tmpcount;
 138                 tmpcount = 0;
 139             }
 140
 141             __asm__ __volatile__ ( REAL_CODE ( "stc\n\t"
 142                                            "sti\n\t"
 143                                            "int $0x13\n\t"
 144                                            "sti\n\t" /* BIOS bugs */
 145                                            "jc 1f\n\t"
 146                                            "xorw %%ax, %%ax\n\t"
 147                                            "\n1:\n\t" )
 148                                : "=a" ( status )
 149                                : "a" ( 0x4200 ), "d" ( VENTOY_BIOS_FAKE_DRIVE ),
 150                                  "S" ( __from_data16 ( &ventoy_address ) ) );
 151         }
 152
 153         curlba += readcount;
 154         left -= readcount;
 155         buffer += (readcount * 2048);
 156     }
 157
 158     start = lba * 2048;
 159     if (start > g_chain->real_img_size_in_bytes)
 160     {
 161         goto end;
 162     }
 163
 164     end = start + count * 2048;
 165     for (i = 0; i < g_override_chunk_num; i++)
 166     {
 167         override_data = g_override_chunk[i].override_data;
 168         override_start = g_override_chunk[i].img_offset;
 169         override_end = override_start + g_override_chunk[i].override_size;
 170
 171         if (end <= override_start || start >= override_end)
 172         {
 173             continue;
 174         }
 175
 176         if (start <= override_start)
 177         {
 178             if (end <= override_end)
 179             {
 180                 memcpy((char *)databuffer + override_start - start, override_data, end - override_start);
 181             }
 182             else
 183             {
 184                 memcpy((char *)databuffer + override_start - start, override_data, override_end - override_start);
 185             }
 186         }
 187         else
 188         {
 189             if (end <= override_end)
 190             {
 191                 memcpy((char *)databuffer, override_data + start - override_start, end - start);
 192             }
 193             else
 194             {
 195                 memcpy((char *)databuffer, override_data + start - override_start, override_end - start);
 196             }
 197         }
 198     }
 199
 200 end:
 201
 202     return 0;
 203 }
 204
 205 int ventoy_vdisk_read(struct san_device *sandev, uint64_t lba, unsigned int count, unsigned long buffer)
 206 {
 207     uint32_t i, j;
 208     uint64_t curlba;
 209     uint64_t lastlba = 0;
 210     uint32_t lbacount = 0;
 211     unsigned long lastbuffer;
 212     uint64_t readend;
 213     ventoy_virt_chunk *node;
 214     ventoy_sector_flag *cur_flag;
 215     ventoy_sector_flag *sector_flag = g_sector_flag;
 216     struct i386_all_regs *ix86;
 217
 218     if (INT13_EXTENDED_READ != sandev->int13_command)
 219     {
 220         DBGC(sandev, "invalid cmd %u\n", sandev->int13_command);
 221         return 0;
 222     }
 223
 224     ix86 = (struct i386_all_regs *)sandev->x86_regptr;
 225
 226     readend = (lba + count) * 2048;
 227     if (readend < g_chain->real_img_size_in_bytes)
 228     {
 229         ventoy_vdisk_read_real(lba, count, buffer);
 230         ix86->regs.dl = sandev->drive;
 231         return 0;
 232     }
 233
 234     if (count > sizeof(g_sector_flag))
 235     {
 236         sector_flag = (ventoy_sector_flag *)malloc(count * sizeof(ventoy_sector_flag));
 237     }
 238
 239     for (curlba = lba, cur_flag = sector_flag, j = 0; j < count; j++, curlba++, cur_flag++)
 240     {
 241         cur_flag->flag = 0;
 242         for (node = g_virt_chunk, i = 0; i < g_virt_chunk_num; i++, node++)
 243         {
 244             if (curlba >= node->mem_sector_start && curlba < node->mem_sector_end)
 245             {
 246                 memcpy((void *)(buffer + j * 2048),
 247                        (char *)g_virt_chunk + node->mem_sector_offset + (curlba - node->mem_sector_start) * 2048,
 248                        2048);
 249                 cur_flag->flag = 1;
 250                 break;
 251             }
 252             else if (curlba >= node->remap_sector_start && curlba < node->remap_sector_end)
 253             {
 254                 cur_flag->remap_lba = node->org_sector_start + curlba - node->remap_sector_start;
 255                 cur_flag->flag = 2;
 256                 break;
 257             }
 258         }
 259     }
 260
 261     for (curlba = lba, cur_flag = sector_flag, j = 0; j < count; j++, curlba++, cur_flag++)
 262     {
 263         if (cur_flag->flag == 2)
 264         {
 265             if (lastlba == 0)
 266             {
 267                 lastbuffer = buffer + j * 2048;
 268                 lastlba = cur_flag->remap_lba;
 269                 lbacount = 1;
 270             }
 271             else if (lastlba + lbacount == cur_flag->remap_lba)
 272             {
 273                 lbacount++;
 274             }
 275             else
 276             {
 277                 ventoy_vdisk_read_real(lastlba, lbacount, lastbuffer);
 278                 lastbuffer = buffer + j * 2048;
 279                 lastlba = cur_flag->remap_lba;
 280                 lbacount = 1;
 281             }
 282         }
 283     }
 284
 285     if (lbacount > 0)
 286     {
 287         ventoy_vdisk_read_real(lastlba, lbacount, lastbuffer);
 288     }
 289
 290     if (sector_flag != g_sector_flag)
 291     {
 292         free(sector_flag);
 293     }
 294
 295     ix86->regs.dl = sandev->drive;
 296     return 0;
 297 }
 298
 299 static void ventoy_dump_img_chunk(ventoy_chain_head *chain)
 300 {
 301     uint32_t i;
 302     ventoy_img_chunk *chunk;
 303
 304     chunk = (ventoy_img_chunk *)((char *)chain + chain->img_chunk_offset);
 305
 306     printf("##################### ventoy_dump_img_chunk #######################\n");
 307
 308     for (i = 0; i < chain->img_chunk_num; i++)
 309     {
 310         printf("%2u: [ %u - %u ] <==> [ %llu - %llu ]\n",
 311                i, chunk[i].img_start_sector, chunk[i].img_end_sector,
 312                chunk[i].disk_start_sector, chunk[i].disk_end_sector);
 313     }
 314
 315     ventoy_debug_pause();
 316 }
 317
 318 static void ventoy_dump_override_chunk(ventoy_chain_head *chain)
 319 {
 320     uint32_t i;
 321     ventoy_override_chunk *chunk;
 322
 323     chunk = (ventoy_override_chunk *)((char *)chain + chain->override_chunk_offset);
 324
 325     printf("##################### ventoy_dump_override_chunk #######################\n");
 326
 327     for (i = 0; i < g_override_chunk_num; i++)
 328     {
 329         printf("%2u: [ %llu, %u ]\n", i, chunk[i].img_offset, chunk[i].override_size);
 330     }
 331
 332     ventoy_debug_pause();
 333 }
 334
 335 static void ventoy_dump_virt_chunk(ventoy_chain_head *chain)
 336 {
 337     uint32_t i;
 338     ventoy_virt_chunk *node;
 339
 340     printf("##################### ventoy_dump_virt_chunk #######################\n");
 341     printf("virt_chunk_offset=%u\n", chain->virt_chunk_offset);
 342     printf("virt_chunk_num=%u\n",    chain->virt_chunk_num);
 343
 344     node = (ventoy_virt_chunk *)((char *)chain + chain->virt_chunk_offset);
 345     for (i = 0; i < chain->virt_chunk_num; i++, node++)
 346     {
 347         printf("%2u: mem:[ %u, %u, %u ]  remap:[ %u, %u, %u ]\n", i,
 348                node->mem_sector_start,
 349                node->mem_sector_end,
 350                node->mem_sector_offset,
 351                node->remap_sector_start,
 352                node->remap_sector_end,
 353                node->org_sector_start);
 354     }
 355
 356     ventoy_debug_pause();
 357 }
 358
 359 static void ventoy_dump_chain(ventoy_chain_head *chain)
 360 {
 361     uint32_t i = 0;
 362     uint8_t chksum = 0;
 363     uint8_t *guid;
 364
 365     guid = chain->os_param.vtoy_disk_guid;
 366     for (i = 0; i < sizeof(ventoy_os_param); i++)
 367     {
 368         chksum += *((uint8_t *)(&(chain->os_param)) + i);
 369     }
 370
 371     printf("##################### ventoy_dump_chain #######################\n");
 372
 373     printf("os_param will be save at %p\n", ventoy_get_runtime_addr());
 374
 375     printf("os_param->chksum=0x%x (%s)\n", chain->os_param.chksum, chksum ? "FAILED" : "SUCCESS");
 376     printf("os_param->vtoy_disk_guid=%02x%02x%02x%02x\n", guid[0], guid[1], guid[2], guid[3]);
 377     printf("os_param->vtoy_disk_size=%llu\n",     chain->os_param.vtoy_disk_size);
 378     printf("os_param->vtoy_disk_part_id=%u\n",    chain->os_param.vtoy_disk_part_id);
 379     printf("os_param->vtoy_disk_part_type=%u\n",  chain->os_param.vtoy_disk_part_type);
 380     printf("os_param->vtoy_img_path=<%s>\n",      chain->os_param.vtoy_img_path);
 381     printf("os_param->vtoy_img_size=<%llu>\n",    chain->os_param.vtoy_img_size);
 382     printf("os_param->vtoy_img_location_addr=<0x%llx>\n", chain->os_param.vtoy_img_location_addr);
 383     printf("os_param->vtoy_img_location_len=<%u>\n",   chain->os_param.vtoy_img_location_len);
 384     ventoy_debug_pause();
 385
 386     printf("chain->disk_drive=0x%x\n",          chain->disk_drive);
 387     printf("chain->drive_map=0x%x\n",           chain->drive_map);
 388     printf("chain->disk_sector_size=%u\n",      chain->disk_sector_size);
 389     printf("chain->real_img_size_in_bytes=%llu\n",   chain->real_img_size_in_bytes);
 390     printf("chain->virt_img_size_in_bytes=%llu\n", chain->virt_img_size_in_bytes);
 391     printf("chain->boot_catalog=%u\n",          chain->boot_catalog);
 392     printf("chain->img_chunk_offset=%u\n",      chain->img_chunk_offset);
 393     printf("chain->img_chunk_num=%u\n",         chain->img_chunk_num);
 394     printf("chain->override_chunk_offset=%u\n", chain->override_chunk_offset);
 395     printf("chain->override_chunk_num=%u\n",    chain->override_chunk_num);
 396     printf("chain->virt_chunk_offset=%u\n",    chain->virt_chunk_offset);
 397     printf("chain->virt_chunk_num=%u\n",    chain->virt_chunk_num);
 398     ventoy_debug_pause();
 399
 400     ventoy_dump_img_chunk(chain);
 401     ventoy_dump_override_chunk(chain);
 402     ventoy_dump_virt_chunk(chain);
 403 }
 404
 405 static int ventoy_update_image_location(ventoy_os_param *param)
 406 {
 407     uint8_t chksum = 0;
 408     unsigned int i;
 409     unsigned int length;
 410     userptr_t address = 0;
 411     ventoy_image_location *location = NULL;
 412     ventoy_image_disk_region *region = NULL;
 413     ventoy_img_chunk *chunk = g_chunk;
 414
 415     length = sizeof(ventoy_image_location) + (g_img_chunk_num - 1) * sizeof(ventoy_image_disk_region);
 416
 417     address = umalloc(length + 4096 * 2);
 418     if (!address)
 419     {
 420         return 0;
 421     }
 422
 423     if (address % 4096)
 424     {
 425         address += 4096 - (address % 4096);
 426     }
 427
 428     param->chksum = 0;
 429     param->vtoy_img_location_addr = user_to_phys(address, 0);
 430     param->vtoy_img_location_len = length;
 431
 432     /* update check sum */
 433     for (i = 0; i < sizeof(ventoy_os_param); i++)
 434     {
 435         chksum += *((uint8_t *)param + i);
 436     }
 437     param->chksum = (chksum == 0) ? 0 : (uint8_t)(0x100 - chksum);
 438
 439     location = (ventoy_image_location *)(unsigned long)(address);
 440     if (NULL == location)
 441     {
 442         return 0;
 443     }
 444
 445     memcpy(&location->guid, &param->guid, sizeof(ventoy_guid));
 446     location->image_sector_size = 2048;
 447     location->disk_sector_size  = g_chain->disk_sector_size;
 448     location->region_count = g_img_chunk_num;
 449
 450     region = location->regions;
 451
 452     for (i = 0; i < g_img_chunk_num; i++)
 453     {
 454         region->image_sector_count = chunk->img_end_sector - chunk->img_start_sector + 1;
 455         region->image_start_sector = chunk->img_start_sector;
 456         region->disk_start_sector  = chunk->disk_start_sector;
 457         region++;
 458         chunk++;
 459     }
 460
 461     return 0;
 462 }
 463
 464 int ventoy_boot_vdisk(void *data)
 465 {
 466     uint8_t chksum = 0;
 467     unsigned int i;
 468     unsigned int drive;
 469
 470     (void)data;
 471
 472     ventoy_address.bufsize = offsetof ( typeof ( ventoy_address ), buffer_phys );
 473
 474     if (strstr(g_cmdline_copy, "debug"))
 475     {
 476         g_debug = 1;
 477         printf("### ventoy chain boot begin... ###\n");
 478         ventoy_debug_pause();
 479     }
 480
 481     g_chain = (ventoy_chain_head *)g_initrd_addr;
 482     g_chunk = (ventoy_img_chunk *)((char *)g_chain + g_chain->img_chunk_offset);
 483     g_img_chunk_num = g_chain->img_chunk_num;
 484     g_disk_sector_size = g_chain->disk_sector_size;
 485     g_cur_chunk = g_chunk;
 486
 487     g_override_chunk = (ventoy_override_chunk *)((char *)g_chain + g_chain->override_chunk_offset);
 488     g_override_chunk_num = g_chain->override_chunk_num;
 489
 490     g_virt_chunk = (ventoy_virt_chunk *)((char *)g_chain + g_chain->virt_chunk_offset);
 491     g_virt_chunk_num = g_chain->virt_chunk_num;
 492
 493     if (g_debug)
 494     {
 495         for (i = 0; i < sizeof(ventoy_os_param); i++)
 496         {
 497             chksum += *((uint8_t *)(&(g_chain->os_param)) + i);
 498         }
 499         printf("os param checksum: 0x%x %s\n", g_chain->os_param.chksum, chksum ? "FAILED" : "SUCCESS");
 500     }
 501
 502     ventoy_update_image_location(&(g_chain->os_param));
 503
 504     if (g_debug)
 505     {
 506         ventoy_dump_chain(g_chain);
 507     }
 508
 509     drive = ventoy_int13_hook(g_chain);
 510
 511     if (g_debug)
 512     {
 513         printf("### ventoy chain boot before boot image ... ###\n");
 514         ventoy_debug_pause();
 515     }
 516
 517     ventoy_int13_boot(drive, &(g_chain->os_param), g_cmdline_copy);
 518
 519     if (g_debug)
 520     {
 521         printf("!!!!!!!!!! ventoy boot failed !!!!!!!!!!\n");
 522         ventoy_debug_pause();
 523     }
 524
 525     return 0;
 526 }
 527