initial commit

IPXE/README.txt

+
+========== About Source Code =============
+1. unpack ipxe_org_code/ipxe-3fe683e.tar.bz2
+2. After decompressing, delete ipxe-3fe683e/src/drivers (whole directory)
+3. Merge left source code with the ipxe-3fe683e directory here
+
+========== Build =============
+make bin/ipxe.iso 

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

+/*
+ * Copyright (C) 2011 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+/** @file
+ *
+ * Command line and initrd passed to iPXE at runtime
+ *
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <errno.h>
+#include <assert.h>
+#include <ipxe/init.h>
+#include <ipxe/image.h>
+#include <ipxe/script.h>
+#include <ipxe/umalloc.h>
+#include <realmode.h>
+#include <ventoy.h>
+
+/** Command line physical address
+ *
+ * This can be set by the prefix.
+ */
+uint32_t __bss16 ( cmdline_phys );
+#define cmdline_phys __use_data16 ( cmdline_phys )
+
+/** initrd physical address
+ *
+ * This can be set by the prefix.
+ */
+uint32_t __bss16 ( initrd_phys );
+#define initrd_phys __use_data16 ( initrd_phys )
+
+/** initrd length
+ *
+ * This can be set by the prefix.
+ */
+uint32_t __bss16 ( initrd_len );
+#define initrd_len __use_data16 ( initrd_len )
+
+/** Internal copy of the command line */
+static char *cmdline_copy;
+
+/** Free command line image */
+static void cmdline_image_free ( struct refcnt *refcnt ) {
+	struct image *image = container_of ( refcnt, struct image, refcnt );
+
+	DBGC ( image, "RUNTIME freeing command line\n" );
+	free ( cmdline_copy );
+}
+
+/** Embedded script representing the command line */
+static struct image cmdline_image = {
+	.refcnt = REF_INIT ( cmdline_image_free ),
+	.name = "<CMDLINE>",
+	.type = &script_image_type,
+};
+
+/** Colour for debug messages */
+#define colour &cmdline_image
+
+/**
+ * Strip unwanted cruft from command line
+ *
+ * @v cmdline		Command line
+ * @v cruft		Initial substring of cruft to strip
+ */
+static void cmdline_strip ( char *cmdline, const char *cruft ) {
+	char *strip;
+	char *strip_end;
+
+	/* Find unwanted cruft, if present */
+	if ( ! ( strip = strstr ( cmdline, cruft ) ) )
+		return;
+
+	/* Strip unwanted cruft */
+	strip_end = strchr ( strip, ' ' );
+	if ( strip_end ) {
+		*strip_end = '\0';
+		DBGC ( colour, "RUNTIME stripping \"%s\"\n", strip );
+		strcpy ( strip, ( strip_end + 1 ) );
+	} else {
+		DBGC ( colour, "RUNTIME stripping \"%s\"\n", strip );
+		*strip = '\0';
+	}
+}
+
+/**
+ * Initialise command line
+ *
+ * @ret rc		Return status code
+ */
+static int cmdline_init ( void ) {
+	userptr_t cmdline_user;
+	char *cmdline;
+	size_t len;
+	int rc;
+
+	/* Do nothing if no command line was specified */
+	if ( ! cmdline_phys ) {
+		DBGC ( colour, "RUNTIME found no command line\n" );
+		return 0;
+	}
+	cmdline_user = phys_to_user ( cmdline_phys );
+	len = ( strlen_user ( cmdline_user, 0 ) + 1 /* NUL */ );
+
+	/* Allocate and copy command line */
+	cmdline_copy = malloc ( len );
+	if ( ! cmdline_copy ) {
+		DBGC ( colour, "RUNTIME could not allocate %zd bytes for "
+		       "command line\n", len );
+		rc = -ENOMEM;
+		goto err_alloc_cmdline_copy;
+	}
+	cmdline = cmdline_copy;
+	copy_from_user ( cmdline, cmdline_user, 0, len );
+	DBGC ( colour, "RUNTIME found command line \"%s\" at %08x\n",
+	       cmdline, cmdline_phys );
+
+	/* Mark command line as consumed */
+	cmdline_phys = 0;
+
+	/* Strip unwanted cruft from the command line */
+	cmdline_strip ( cmdline, "BOOT_IMAGE=" );
+	cmdline_strip ( cmdline, "initrd=" );
+	while ( isspace ( *cmdline ) )
+		cmdline++;
+	DBGC ( colour, "RUNTIME using command line \"%s\"\n", cmdline );
+
+	/* Prepare and register image */
+	cmdline_image.data = virt_to_user ( cmdline );
+	cmdline_image.len = strlen ( cmdline );
+	if ( cmdline_image.len ) {
+		if ( ( rc = register_image ( &cmdline_image ) ) != 0 ) {
+			DBGC ( colour, "RUNTIME could not register command "
+			       "line: %s\n", strerror ( rc ) );
+			goto err_register_image;
+		}
+	}
+
+	/* Drop our reference to the image */
+	image_put ( &cmdline_image );
+
+	return 0;
+
+ err_register_image:
+	image_put ( &cmdline_image );
+ err_alloc_cmdline_copy:
+	return rc;
+}
+
+/**
+ * Initialise initrd
+ *
+ * @ret rc		Return status code
+ */
+static int initrd_init ( void ) {
+	struct image *image;
+	int rc;
+
+	/* Do nothing if no initrd was specified */
+	if ( ! initrd_phys ) {
+		DBGC ( colour, "RUNTIME found no initrd\n" );
+		return 0;
+	}
+	if ( ! initrd_len ) {
+		DBGC ( colour, "RUNTIME found empty initrd\n" );
+		return 0;
+	}
+	DBGC ( colour, "RUNTIME found initrd at [%x,%x)\n",
+	       initrd_phys, ( initrd_phys + initrd_len ) );
+
+	/* Allocate image */
+	image = alloc_image ( NULL );
+	if ( ! image ) {
+		DBGC ( colour, "RUNTIME could not allocate image for "
+		       "initrd\n" );
+		rc = -ENOMEM;
+		goto err_alloc_image;
+	}
+	if ( ( rc = image_set_name ( image, "<INITRD>" ) ) != 0 ) {
+		DBGC ( colour, "RUNTIME could not set image name: %s\n",
+		       strerror ( rc ) );
+		goto err_set_name;
+	}
+
+	/* Allocate and copy initrd content */
+	image->data = umalloc ( initrd_len );
+	if ( ! image->data ) {
+		DBGC ( colour, "RUNTIME could not allocate %d bytes for "
+		       "initrd\n", initrd_len );
+		rc = -ENOMEM;
+		goto err_umalloc;
+	}
+	image->len = initrd_len;
+	memcpy_user ( image->data, 0, phys_to_user ( initrd_phys ), 0,
+		      initrd_len );
+
+    g_initrd_addr = (void *)image->data;  
+    g_initrd_len = image->len;  
+    g_cmdline_copy = cmdline_copy;
+
+
+	/* Mark initrd as consumed */
+	initrd_phys = 0;
+
+	/* Register image */
+	if ( ( rc = register_image ( image ) ) != 0 ) {
+		DBGC ( colour, "RUNTIME could not register initrd: %s\n",
+		       strerror ( rc ) );
+		goto err_register_image;
+	}
+
+	/* Drop our reference to the image */
+	image_put ( image );
+
+	return 0;
+
+ err_register_image:
+ err_umalloc:
+ err_set_name:
+	image_put ( image );
+ err_alloc_image:
+	return rc;
+}
+
+/**
+ * Initialise command line and initrd
+ *
+ */
+static void runtime_init ( void ) {
+	int rc;
+
+	/* Initialise command line */
+	if ( ( rc = cmdline_init() ) != 0 ) {
+		/* No way to report failure */
+		return;
+	}
+
+	/* Initialise initrd */
+	if ( ( rc = initrd_init() ) != 0 ) {
+		/* No way to report failure */
+		return;
+	}
+}
+
+/** Command line and initrd initialisation function */
+struct startup_fn runtime_startup_fn __startup_fn ( STARTUP_NORMAL ) = {
+	.name = "runtime",
+	.startup = runtime_init,
+};

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

+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <limits.h>
+#include <byteswap.h>
+#include <errno.h>
+#include <assert.h>
+#include <ipxe/blockdev.h>
+#include <ipxe/io.h>
+#include <ipxe/acpi.h>
+#include <ipxe/sanboot.h>
+#include <ipxe/device.h>
+#include <ipxe/pci.h>
+#include <ipxe/eltorito.h>
+#include <ipxe/timer.h>
+#include <ipxe/umalloc.h>
+#include <realmode.h>
+#include <bios.h>
+#include <biosint.h>
+#include <bootsector.h>
+#include <int13.h>
+#include <ventoy.h>
+
+int g_debug = 0;
+char *g_cmdline_copy;
+void *g_initrd_addr;
+size_t g_initrd_len;
+ventoy_chain_head *g_chain;
+ventoy_img_chunk *g_chunk;
+uint32_t g_img_chunk_num;
+ventoy_img_chunk *g_cur_chunk;
+uint32_t g_disk_sector_size;
+
+ventoy_override_chunk *g_override_chunk;
+uint32_t g_override_chunk_num;
+
+ventoy_virt_chunk *g_virt_chunk;
+uint32_t g_virt_chunk_num;
+
+ventoy_sector_flag g_sector_flag[128];
+
+static struct int13_disk_address __bss16 ( ventoy_address );
+#define ventoy_address __use_data16 ( ventoy_address )
+
+static uint64_t ventoy_remap_lba(uint64_t lba, uint32_t *count)
+{
+    uint32_t i;
+    uint32_t max_sectors;
+    ventoy_img_chunk *cur;
+
+    if ((NULL == g_cur_chunk) || ((lba) < g_cur_chunk->img_start_sector) || ((lba) > g_cur_chunk->img_end_sector))
+    {
+        g_cur_chunk = NULL;
+        for (i = 0; i < g_img_chunk_num; i++)
+        {
+            cur = g_chunk + i;
+            if (lba >= cur->img_start_sector && lba <= cur->img_end_sector)
+            {
+                g_cur_chunk = cur;
+                break;
+            }
+        }
+    }
+
+    if (g_cur_chunk)
+    {
+        max_sectors = g_cur_chunk->img_end_sector - lba + 1;
+        if (*count > max_sectors)
+        {
+            *count = max_sectors;
+        }
+
+        if (512 == g_disk_sector_size)
+        {
+            return g_cur_chunk->disk_start_sector + ((lba - g_cur_chunk->img_start_sector) << 2);            
+        }
+        return g_cur_chunk->disk_start_sector + (lba - g_cur_chunk->img_start_sector) * 2048 / g_disk_sector_size;
+    }
+    return lba;
+}
+
+static int ventoy_vdisk_read_real(uint64_t lba, unsigned int count, unsigned long buffer)
+{
+    uint32_t i = 0;
+    uint32_t left = 0;
+    uint32_t readcount = 0;
+    uint32_t tmpcount = 0;
+    uint16_t status = 0;
+    uint64_t curlba = 0;
+    uint64_t maplba = 0;
+    uint64_t start = 0;
+    uint64_t end = 0;
+    uint64_t override_start = 0;
+    uint64_t override_end = 0;
+    unsigned long phyaddr;
+    unsigned long databuffer = buffer;
+    uint8_t *override_data;
+
+    curlba = lba;
+    left = count;
+
+    while (left > 0)
+    {
+        readcount = left;
+        maplba = ventoy_remap_lba(curlba, &readcount);
+        
+        if (g_disk_sector_size == 512)
+        {
+            tmpcount = (readcount << 2);
+        }
+        else
+        {
+            tmpcount = (readcount * 2048) / g_disk_sector_size;
+        }
+
+        phyaddr = user_to_phys(buffer, 0);
+
+        while (tmpcount > 0)
+        {
+            /* Use INT 13, 42 to read the data from real disk */
+            ventoy_address.lba = maplba;
+            ventoy_address.buffer.segment = (uint16_t)(phyaddr >> 4);
+    	    ventoy_address.buffer.offset = (uint16_t)(phyaddr & 0x0F);
+
+            if (tmpcount >= 64) /* max sectors per transmit */
+            {
+                ventoy_address.count = 64;
+                tmpcount -= 64;
+                maplba   += 64;
+                phyaddr  += 32768;
+            }
+            else
+            {
+                ventoy_address.count = tmpcount;
+                tmpcount = 0;
+            }
+
+            __asm__ __volatile__ ( REAL_CODE ( "stc\n\t"
+    					   "sti\n\t"
+    					   "int $0x13\n\t"
+    					   "sti\n\t" /* BIOS bugs */
+    					   "jc 1f\n\t"
+    					   "xorw %%ax, %%ax\n\t"
+    					   "\n1:\n\t" )
+    			       : "=a" ( status )
+    			       : "a" ( 0x4200 ), "d" ( VENTOY_BIOS_FAKE_DRIVE ),
+    				 "S" ( __from_data16 ( &ventoy_address ) ) );
+        }
+
+        curlba += readcount;
+        left -= readcount;
+        buffer += (readcount * 2048);
+    }
+
+    start = lba * 2048;
+    if (start > g_chain->real_img_size_in_bytes)
+    {
+        goto end;
+    }
+
+    end = start + count * 2048;
+    for (i = 0; i < g_override_chunk_num; i++)
+    {
+        override_data = g_override_chunk[i].override_data;
+        override_start = g_override_chunk[i].img_offset;
+        override_end = override_start + g_override_chunk[i].override_size;
+
+        if (end <= override_start || start >= override_end)
+        {
+            continue;
+        }
+
+        if (start <= override_start)
+        {
+            if (end <= override_end)
+            {
+                memcpy((char *)databuffer + override_start - start, override_data, end - override_start);  
+            }
+            else
+            {
+                memcpy((char *)databuffer + override_start - start, override_data, override_end - override_start);
+            }
+        }
+        else
+        {
+            if (end <= override_end)
+            {
+                memcpy((char *)databuffer, override_data + start - override_start, end - start);     
+            }
+            else
+            {
+                memcpy((char *)databuffer, override_data + start - override_start, override_end - start);
+            }
+        }
+    }
+
+end:
+
+    return 0;
+}
+
+int ventoy_vdisk_read(struct san_device *sandev, uint64_t lba, unsigned int count, unsigned long buffer)
+{
+    uint32_t i, j;
+    uint64_t curlba;
+    uint64_t lastlba = 0;
+    uint32_t lbacount = 0;
+    unsigned long lastbuffer;
+    uint64_t readend;
+    ventoy_virt_chunk *node;
+    ventoy_sector_flag *cur_flag;
+    ventoy_sector_flag *sector_flag = g_sector_flag;
+    struct i386_all_regs *ix86;
+
+    if (INT13_EXTENDED_READ != sandev->int13_command)
+    {
+        DBGC(sandev, "invalid cmd %u\n", sandev->int13_command);
+        return 0;
+    }
+
+    ix86 = (struct i386_all_regs *)sandev->x86_regptr;
+
+    readend = (lba + count) * 2048;
+    if (readend < g_chain->real_img_size_in_bytes)
+    {
+        ventoy_vdisk_read_real(lba, count, buffer);
+        ix86->regs.dl = sandev->drive;
+        return 0;
+    }
+
+    if (count > sizeof(g_sector_flag))
+    {
+        sector_flag = (ventoy_sector_flag *)malloc(count * sizeof(ventoy_sector_flag));
+    }
+
+    for (curlba = lba, cur_flag = sector_flag, j = 0; j < count; j++, curlba++, cur_flag++)
+    {
+        cur_flag->flag = 0;
+        for (node = g_virt_chunk, i = 0; i < g_virt_chunk_num; i++, node++)
+        {
+            if (curlba >= node->mem_sector_start && curlba < node->mem_sector_end)
+            {
+                memcpy((void *)(buffer + j * 2048), 
+                       (char *)g_virt_chunk + node->mem_sector_offset + (curlba - node->mem_sector_start) * 2048,
+                       2048);
+                cur_flag->flag = 1;
+                break;
+            }
+            else if (curlba >= node->remap_sector_start && curlba < node->remap_sector_end)
+            {
+                cur_flag->remap_lba = node->org_sector_start + curlba - node->remap_sector_start;
+                cur_flag->flag = 2;
+                break;
+            }
+        }
+    }
+
+    for (curlba = lba, cur_flag = sector_flag, j = 0; j < count; j++, curlba++, cur_flag++)
+    {
+        if (cur_flag->flag == 2)
+        {
+            if (lastlba == 0)
+            {
+                lastbuffer = buffer + j * 2048;
+                lastlba = cur_flag->remap_lba;
+                lbacount = 1;
+            }
+            else if (lastlba + lbacount == cur_flag->remap_lba)
+            {
+                lbacount++;
+            }
+            else
+            {
+                ventoy_vdisk_read_real(lastlba, lbacount, lastbuffer);
+                lastbuffer = buffer + j * 2048;
+                lastlba = cur_flag->remap_lba;
+                lbacount = 1;
+            }
+        }
+    }
+
+    if (lbacount > 0)
+    {
+        ventoy_vdisk_read_real(lastlba, lbacount, lastbuffer);
+    }
+
+    if (sector_flag != g_sector_flag)
+    {
+        free(sector_flag);
+    }
+
+    ix86->regs.dl = sandev->drive;
+    return 0;
+}
+
+static void ventoy_dump_img_chunk(ventoy_chain_head *chain)
+{
+    uint32_t i;
+    ventoy_img_chunk *chunk;
+
+    chunk = (ventoy_img_chunk *)((char *)chain + chain->img_chunk_offset);
+
+    printf("##################### ventoy_dump_img_chunk #######################\n");
+
+    for (i = 0; i < chain->img_chunk_num; i++)
+    {
+        printf("%2u: [ %u - %u ] <==> [ %llu - %llu ]\n",
+               i, chunk[i].img_start_sector, chunk[i].img_end_sector, 
+               chunk[i].disk_start_sector, chunk[i].disk_end_sector);
+    }
+    
+    ventoy_debug_pause();
+}
+
+static void ventoy_dump_override_chunk(ventoy_chain_head *chain)
+{
+    uint32_t i;
+    ventoy_override_chunk *chunk;
+    
+    chunk = (ventoy_override_chunk *)((char *)chain + chain->override_chunk_offset);
+
+    printf("##################### ventoy_dump_override_chunk #######################\n");
+
+    for (i = 0; i < g_override_chunk_num; i++)
+    {
+        printf("%2u: [ %llu, %u ]\n", i, chunk[i].img_offset, chunk[i].override_size);
+    }
+    
+    ventoy_debug_pause();
+}
+
+static void ventoy_dump_virt_chunk(ventoy_chain_head *chain)
+{
+    uint32_t i;
+    ventoy_virt_chunk *node;
+     
+    printf("##################### ventoy_dump_virt_chunk #######################\n");
+    printf("virt_chunk_offset=%u\n", chain->virt_chunk_offset);
+    printf("virt_chunk_num=%u\n",    chain->virt_chunk_num);
+
+    node = (ventoy_virt_chunk *)((char *)chain + chain->virt_chunk_offset);
+    for (i = 0; i < chain->virt_chunk_num; i++, node++)
+    {
+        printf("%2u: mem:[ %u, %u, %u ]  remap:[ %u, %u, %u ]\n", i, 
+               node->mem_sector_start,
+               node->mem_sector_end,
+               node->mem_sector_offset,
+               node->remap_sector_start,
+               node->remap_sector_end,
+               node->org_sector_start);
+    }
+    
+    ventoy_debug_pause();
+}
+
+static void ventoy_dump_chain(ventoy_chain_head *chain)
+{
+    uint32_t i = 0;
+    uint8_t chksum = 0;
+    uint8_t *guid;
+    
+    guid = chain->os_param.vtoy_disk_guid;
+    for (i = 0; i < sizeof(ventoy_os_param); i++)
+    {
+        chksum += *((uint8_t *)(&(chain->os_param)) + i);
+    }
+
+    printf("##################### ventoy_dump_chain #######################\n");
+
+    printf("os_param will be save at %p\n", ventoy_get_runtime_addr());
+
+    printf("os_param->chksum=0x%x (%s)\n", chain->os_param.chksum, chksum ? "FAILED" : "SUCCESS");
+    printf("os_param->vtoy_disk_guid=%02x%02x%02x%02x\n", guid[0], guid[1], guid[2], guid[3]);
+    printf("os_param->vtoy_disk_size=%llu\n",     chain->os_param.vtoy_disk_size);
+    printf("os_param->vtoy_disk_part_id=%u\n",    chain->os_param.vtoy_disk_part_id);
+    printf("os_param->vtoy_disk_part_type=%u\n",  chain->os_param.vtoy_disk_part_type);
+    printf("os_param->vtoy_img_path=<%s>\n",      chain->os_param.vtoy_img_path);
+    printf("os_param->vtoy_img_size=<%llu>\n",    chain->os_param.vtoy_img_size);
+    printf("os_param->vtoy_img_location_addr=<0x%llx>\n", chain->os_param.vtoy_img_location_addr);
+    printf("os_param->vtoy_img_location_len=<%u>\n",   chain->os_param.vtoy_img_location_len);
+    ventoy_debug_pause();
+
+    printf("chain->disk_drive=0x%x\n",          chain->disk_drive);
+    printf("chain->drive_map=0x%x\n",           chain->drive_map);
+    printf("chain->disk_sector_size=%u\n",      chain->disk_sector_size);
+    printf("chain->real_img_size_in_bytes=%llu\n",   chain->real_img_size_in_bytes);
+    printf("chain->virt_img_size_in_bytes=%llu\n", chain->virt_img_size_in_bytes);
+    printf("chain->boot_catalog=%u\n",          chain->boot_catalog);
+    printf("chain->img_chunk_offset=%u\n",      chain->img_chunk_offset);
+    printf("chain->img_chunk_num=%u\n",         chain->img_chunk_num);
+    printf("chain->override_chunk_offset=%u\n", chain->override_chunk_offset);
+    printf("chain->override_chunk_num=%u\n",    chain->override_chunk_num);
+    printf("chain->virt_chunk_offset=%u\n",    chain->virt_chunk_offset);
+    printf("chain->virt_chunk_num=%u\n",    chain->virt_chunk_num);
+    ventoy_debug_pause();
+
+    ventoy_dump_img_chunk(chain);
+    ventoy_dump_override_chunk(chain);
+    ventoy_dump_virt_chunk(chain);
+}
+
+static int ventoy_update_image_location(ventoy_os_param *param)
+{
+    uint8_t chksum = 0;
+    unsigned int i;
+    unsigned int length;
+    userptr_t address = 0;
+    ventoy_image_location *location = NULL;
+    ventoy_image_disk_region *region = NULL;
+    ventoy_img_chunk *chunk = g_chunk;
+
+    length = sizeof(ventoy_image_location) + (g_img_chunk_num - 1) * sizeof(ventoy_image_disk_region);
+
+    address = umalloc(length + 4096 * 2);
+    if (!address)
+    {
+        return 0;
+    }
+
+    if (address % 4096)
+    {
+        address += 4096 - (address % 4096);
+    }
+
+    param->chksum = 0;
+    param->vtoy_img_location_addr = user_to_phys(address, 0);
+    param->vtoy_img_location_len = length;
+
+    /* update check sum */
+    for (i = 0; i < sizeof(ventoy_os_param); i++)
+    {
+        chksum += *((uint8_t *)param + i);
+    }
+    param->chksum = (chksum == 0) ? 0 : (uint8_t)(0x100 - chksum);
+
+    location = (ventoy_image_location *)(unsigned long)(address);
+    if (NULL == location)
+    {
+        return 0;
+    }
+    
+    memcpy(&location->guid, &param->guid, sizeof(ventoy_guid));
+    location->image_sector_size = 2048;
+    location->disk_sector_size  = g_chain->disk_sector_size;
+    location->region_count = g_img_chunk_num;
+
+    region = location->regions;
+
+    for (i = 0; i < g_img_chunk_num; i++)
+    {
+        region->image_sector_count = chunk->img_end_sector - chunk->img_start_sector + 1;
+        region->image_start_sector = chunk->img_start_sector;
+        region->disk_start_sector  = chunk->disk_start_sector;
+        region++;
+        chunk++;
+    }
+
+    return 0;
+}
+
+int ventoy_boot_vdisk(void *data)
+{
+    uint8_t chksum = 0;
+    unsigned int i;
+    unsigned int drive;
+    
+    (void)data;
+
+    ventoy_address.bufsize = offsetof ( typeof ( ventoy_address ), buffer_phys );
+
+    if (strstr(g_cmdline_copy, "debug"))
+    {
+        g_debug = 1;
+        printf("### ventoy chain boot begin... ###\n");
+        ventoy_debug_pause();
+    }
+
+    g_chain = (ventoy_chain_head *)g_initrd_addr;
+    g_chunk = (ventoy_img_chunk *)((char *)g_chain + g_chain->img_chunk_offset);
+    g_img_chunk_num = g_chain->img_chunk_num;
+    g_disk_sector_size = g_chain->disk_sector_size;
+    g_cur_chunk = g_chunk;
+
+    g_override_chunk = (ventoy_override_chunk *)((char *)g_chain + g_chain->override_chunk_offset);
+    g_override_chunk_num = g_chain->override_chunk_num;
+
+    g_virt_chunk = (ventoy_virt_chunk *)((char *)g_chain + g_chain->virt_chunk_offset);
+    g_virt_chunk_num = g_chain->virt_chunk_num;
+
+    if (g_debug)
+    {
+        for (i = 0; i < sizeof(ventoy_os_param); i++)
+        {
+            chksum += *((uint8_t *)(&(g_chain->os_param)) + i);
+        }
+        printf("os param checksum: 0x%x %s\n", g_chain->os_param.chksum, chksum ? "FAILED" : "SUCCESS");
+    }
+
+    ventoy_update_image_location(&(g_chain->os_param));
+
+    if (g_debug)
+    {
+        ventoy_dump_chain(g_chain);
+    }
+
+    drive = ventoy_int13_hook(g_chain);
+
+    if (g_debug)
+    {
+        printf("### ventoy chain boot before boot image ... ###\n");
+        ventoy_debug_pause();    
+    }
+    
+    ventoy_int13_boot(drive, &(g_chain->os_param), g_cmdline_copy);
+
+    if (g_debug)
+    {
+        printf("!!!!!!!!!! ventoy boot failed !!!!!!!!!!\n");
+        ventoy_debug_pause();
+    }
+
+    return 0;
+}
+

IPXE/ipxe-3fe683e/src/arch/x86/drivers/hyperv/hyperv.c

+/*
+ * Copyright (C) 2014 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+/** @file
+ *
+ * Hyper-V driver
+ *
+ */
+
+#include <stdlib.h>
+#include <stdarg.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+#include <errno.h>
+#include <byteswap.h>
+#include <pic8259.h>
+#include <ipxe/malloc.h>
+#include <ipxe/device.h>
+#include <ipxe/timer.h>
+#include <ipxe/quiesce.h>
+#include <ipxe/cpuid.h>
+#include <ipxe/msr.h>
+#include <ipxe/hyperv.h>
+#include <ipxe/vmbus.h>
+#include "hyperv.h"
+
+/** Maximum time to wait for a message response
+ *
+ * This is a policy decision.
+ */
+#define HV_MESSAGE_MAX_WAIT_MS 1000
+
+/** Hyper-V timer frequency (fixed 10Mhz) */
+#define HV_TIMER_HZ 10000000
+
+/** Hyper-V timer scale factor (used to avoid 64-bit division) */
+#define HV_TIMER_SHIFT 18
+
+/**
+ * Convert a Hyper-V status code to an iPXE status code
+ *
+ * @v status		Hyper-V status code
+ * @ret rc		iPXE status code (before negation)
+ */
+#define EHV( status ) EPLATFORM ( EINFO_EPLATFORM, (status) )
+
+/**
+ * Allocate zeroed pages
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v ...		Page addresses to fill in, terminated by NULL
+ * @ret rc		Return status code
+ */
+__attribute__ (( sentinel )) int
+hv_alloc_pages ( struct hv_hypervisor *hv, ... ) {
+	va_list args;
+	void **page;
+	int i;
+
+	/* Allocate and zero pages */
+	va_start ( args, hv );
+	for ( i = 0 ; ( ( page = va_arg ( args, void ** ) ) != NULL ); i++ ) {
+		*page = malloc_dma ( PAGE_SIZE, PAGE_SIZE );
+		if ( ! *page )
+			goto err_alloc;
+		memset ( *page, 0, PAGE_SIZE );
+	}
+	va_end ( args );
+
+	return 0;
+
+ err_alloc:
+	va_end ( args );
+	va_start ( args, hv );
+	for ( ; i >= 0 ; i-- ) {
+		page = va_arg ( args, void ** );
+		free_dma ( *page, PAGE_SIZE );
+	}
+	va_end ( args );
+	return -ENOMEM;
+}
+
+/**
+ * Free pages
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v ...		Page addresses, terminated by NULL
+ */
+__attribute__ (( sentinel )) void
+hv_free_pages ( struct hv_hypervisor *hv, ... ) {
+	va_list args;
+	void *page;
+
+	va_start ( args, hv );
+	while ( ( page = va_arg ( args, void * ) ) != NULL )
+		free_dma ( page, PAGE_SIZE );
+	va_end ( args );
+}
+
+/**
+ * Allocate message buffer
+ *
+ * @v hv		Hyper-V hypervisor
+ * @ret rc		Return status code
+ */
+static int hv_alloc_message ( struct hv_hypervisor *hv ) {
+
+	/* Allocate buffer.  Must be aligned to at least 8 bytes and
+	 * must not cross a page boundary, so align on its own size.
+	 */
+	hv->message = malloc_dma ( sizeof ( *hv->message ),
+				   sizeof ( *hv->message ) );
+	if ( ! hv->message )
+		return -ENOMEM;
+
+	return 0;
+}
+
+/**
+ * Free message buffer
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_free_message ( struct hv_hypervisor *hv ) {
+
+	/* Free buffer */
+	free_dma ( hv->message, sizeof ( *hv->message ) );
+}
+
+/**
+ * Check whether or not we are running in Hyper-V
+ *
+ * @ret rc		Return status code
+ */
+static int hv_check_hv ( void ) {
+	struct x86_features features;
+	uint32_t interface_id;
+	uint32_t discard_ebx;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+
+	/* Check for presence of a hypervisor (not necessarily Hyper-V) */
+	x86_features ( &features );
+	if ( ! ( features.intel.ecx & CPUID_FEATURES_INTEL_ECX_HYPERVISOR ) ) {
+		DBGC ( HV_INTERFACE_ID, "HV not running in a hypervisor\n" );
+		return -ENODEV;
+	}
+
+	/* Check that hypervisor is Hyper-V */
+	cpuid ( HV_CPUID_INTERFACE_ID, 0, &interface_id, &discard_ebx,
+		&discard_ecx, &discard_edx );
+	if ( interface_id != HV_INTERFACE_ID ) {
+		DBGC ( HV_INTERFACE_ID, "HV not running in Hyper-V (interface "
+		       "ID %#08x)\n", interface_id );
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * Check required features
+ *
+ * @v hv		Hyper-V hypervisor
+ * @ret rc		Return status code
+ */
+static int hv_check_features ( struct hv_hypervisor *hv ) {
+	uint32_t available;
+	uint32_t permissions;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+
+	/* Check that required features and privileges are available */
+	cpuid ( HV_CPUID_FEATURES, 0, &available, &permissions, &discard_ecx,
+		&discard_edx );
+	if ( ! ( available & HV_FEATURES_AVAIL_HYPERCALL_MSR ) ) {
+		DBGC ( hv, "HV %p has no hypercall MSRs (features %08x:%08x)\n",
+		       hv, available, permissions );
+		return -ENODEV;
+	}
+	if ( ! ( available & HV_FEATURES_AVAIL_SYNIC_MSR ) ) {
+		DBGC ( hv, "HV %p has no SynIC MSRs (features %08x:%08x)\n",
+		       hv, available, permissions );
+		return -ENODEV;
+	}
+	if ( ! ( permissions & HV_FEATURES_PERM_POST_MESSAGES ) ) {
+		DBGC ( hv, "HV %p cannot post messages (features %08x:%08x)\n",
+		       hv, available, permissions );
+		return -EACCES;
+	}
+	if ( ! ( permissions & HV_FEATURES_PERM_SIGNAL_EVENTS ) ) {
+		DBGC ( hv, "HV %p cannot signal events (features %08x:%08x)",
+		       hv, available, permissions );
+		return -EACCES;
+	}
+
+	return 0;
+}
+
+/**
+ * Check that Gen 2 UEFI firmware is not running
+ *
+ * @v hv		Hyper-V hypervisor
+ * @ret rc		Return status code
+ *
+ * We must not steal ownership from the Gen 2 UEFI firmware, since
+ * doing so will cause an immediate crash.  Avoid this by checking for
+ * the guest OS identity known to be used by the Gen 2 UEFI firmware.
+ */
+static int hv_check_uefi ( struct hv_hypervisor *hv ) {
+	uint64_t guest_os_id;
+
+	/* Check for UEFI firmware's guest OS identity */
+	guest_os_id = rdmsr ( HV_X64_MSR_GUEST_OS_ID );
+	if ( guest_os_id == HV_GUEST_OS_ID_UEFI ) {
+		DBGC ( hv, "HV %p is owned by UEFI firmware\n", hv );
+		return -ENOTSUP;
+	}
+
+	return 0;
+}
+
+/**
+ * Map hypercall page
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_map_hypercall ( struct hv_hypervisor *hv ) {
+	union {
+		struct {
+			uint32_t ebx;
+			uint32_t ecx;
+			uint32_t edx;
+		} __attribute__ (( packed ));
+		char text[ 13 /* "bbbbccccdddd" + NUL */ ];
+	} vendor_id;
+	uint32_t build;
+	uint32_t version;
+	uint32_t discard_eax;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+	uint64_t guest_os_id;
+	uint64_t hypercall;
+
+	/* Report guest OS identity */
+	guest_os_id = rdmsr ( HV_X64_MSR_GUEST_OS_ID );
+	if ( guest_os_id != 0 ) {
+		DBGC ( hv, "HV %p guest OS ID MSR was %#08llx\n",
+		       hv, guest_os_id );
+	}
+	guest_os_id = HV_GUEST_OS_ID_IPXE;
+	DBGC2 ( hv, "HV %p guest OS ID MSR is %#08llx\n", hv, guest_os_id );
+	wrmsr ( HV_X64_MSR_GUEST_OS_ID, guest_os_id );
+
+	/* Get hypervisor system identity (for debugging) */
+	cpuid ( HV_CPUID_VENDOR_ID, 0, &discard_eax, &vendor_id.ebx,
+		&vendor_id.ecx, &vendor_id.edx );
+	vendor_id.text[ sizeof ( vendor_id.text ) - 1 ] = '\0';
+	cpuid ( HV_CPUID_HYPERVISOR_ID, 0, &build, &version, &discard_ecx,
+		&discard_edx );
+	DBGC ( hv, "HV %p detected \"%s\" version %d.%d build %d\n", hv,
+	       vendor_id.text, ( version >> 16 ), ( version & 0xffff ), build );
+
+	/* Map hypercall page */
+	hypercall = rdmsr ( HV_X64_MSR_HYPERCALL );
+	hypercall &= ( PAGE_SIZE - 1 );
+	hypercall |= ( virt_to_phys ( hv->hypercall ) | HV_HYPERCALL_ENABLE );
+	DBGC2 ( hv, "HV %p hypercall MSR is %#08llx\n", hv, hypercall );
+	wrmsr ( HV_X64_MSR_HYPERCALL, hypercall );
+}
+
+/**
+ * Unmap hypercall page
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_unmap_hypercall ( struct hv_hypervisor *hv ) {
+	uint64_t hypercall;
+	uint64_t guest_os_id;
+
+	/* Unmap the hypercall page */
+	hypercall = rdmsr ( HV_X64_MSR_HYPERCALL );
+	hypercall &= ( ( PAGE_SIZE - 1 ) & ~HV_HYPERCALL_ENABLE );
+	DBGC2 ( hv, "HV %p hypercall MSR is %#08llx\n", hv, hypercall );
+	wrmsr ( HV_X64_MSR_HYPERCALL, hypercall );
+
+	/* Reset the guest OS identity */
+	guest_os_id = 0;
+	DBGC2 ( hv, "HV %p guest OS ID MSR is %#08llx\n", hv, guest_os_id );
+	wrmsr ( HV_X64_MSR_GUEST_OS_ID, guest_os_id );
+}
+
+/**
+ * Map synthetic interrupt controller
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_map_synic ( struct hv_hypervisor *hv ) {
+	uint64_t simp;
+	uint64_t siefp;
+	uint64_t scontrol;
+
+	/* Zero SynIC message and event pages */
+	memset ( hv->synic.message, 0, PAGE_SIZE );
+	memset ( hv->synic.event, 0, PAGE_SIZE );
+
+	/* Map SynIC message page */
+	simp = rdmsr ( HV_X64_MSR_SIMP );
+	simp &= ( PAGE_SIZE - 1 );
+	simp |= ( virt_to_phys ( hv->synic.message ) | HV_SIMP_ENABLE );
+	DBGC2 ( hv, "HV %p SIMP MSR is %#08llx\n", hv, simp );
+	wrmsr ( HV_X64_MSR_SIMP, simp );
+
+	/* Map SynIC event page */
+	siefp = rdmsr ( HV_X64_MSR_SIEFP );
+	siefp &= ( PAGE_SIZE - 1 );
+	siefp |= ( virt_to_phys ( hv->synic.event ) | HV_SIEFP_ENABLE );
+	DBGC2 ( hv, "HV %p SIEFP MSR is %#08llx\n", hv, siefp );
+	wrmsr ( HV_X64_MSR_SIEFP, siefp );
+
+	/* Enable SynIC */
+	scontrol = rdmsr ( HV_X64_MSR_SCONTROL );
+	scontrol |= HV_SCONTROL_ENABLE;
+	DBGC2 ( hv, "HV %p SCONTROL MSR is %#08llx\n", hv, scontrol );
+	wrmsr ( HV_X64_MSR_SCONTROL, scontrol );
+}
+
+/**
+ * Unmap synthetic interrupt controller, leaving SCONTROL untouched
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_unmap_synic_no_scontrol ( struct hv_hypervisor *hv ) {
+	uint64_t siefp;
+	uint64_t simp;
+
+	/* Unmap SynIC event page */
+	siefp = rdmsr ( HV_X64_MSR_SIEFP );
+	siefp &= ( ( PAGE_SIZE - 1 ) & ~HV_SIEFP_ENABLE );
+	DBGC2 ( hv, "HV %p SIEFP MSR is %#08llx\n", hv, siefp );
+	wrmsr ( HV_X64_MSR_SIEFP, siefp );
+
+	/* Unmap SynIC message page */
+	simp = rdmsr ( HV_X64_MSR_SIMP );
+	simp &= ( ( PAGE_SIZE - 1 ) & ~HV_SIMP_ENABLE );
+	DBGC2 ( hv, "HV %p SIMP MSR is %#08llx\n", hv, simp );
+	wrmsr ( HV_X64_MSR_SIMP, simp );
+}
+
+/**
+ * Unmap synthetic interrupt controller
+ *
+ * @v hv		Hyper-V hypervisor
+ */
+static void hv_unmap_synic ( struct hv_hypervisor *hv ) {
+	uint64_t scontrol;
+
+	/* Disable SynIC */
+	scontrol = rdmsr ( HV_X64_MSR_SCONTROL );
+	scontrol &= ~HV_SCONTROL_ENABLE;
+	DBGC2 ( hv, "HV %p SCONTROL MSR is %#08llx\n", hv, scontrol );
+	wrmsr ( HV_X64_MSR_SCONTROL, scontrol );
+
+	/* Unmap SynIC event and message pages */
+	hv_unmap_synic_no_scontrol ( hv );
+}
+
+/**
+ * Enable synthetic interrupt
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v sintx		Synthetic interrupt number
+ */
+void hv_enable_sint ( struct hv_hypervisor *hv, unsigned int sintx ) {
+	unsigned long msr = HV_X64_MSR_SINT ( sintx );
+	uint64_t sint;
+
+	/* Enable synthetic interrupt
+	 *
+	 * We have to enable the interrupt, otherwise messages will
+	 * not be delivered (even though the documentation implies
+	 * that polling for messages is possible).  We enable AutoEOI
+	 * and hook the interrupt to the obsolete IRQ13 (FPU
+	 * exception) vector, which will be implemented as a no-op.
+	 */
+	sint = rdmsr ( msr );
+	sint &= ~( HV_SINT_MASKED | HV_SINT_VECTOR_MASK );
+	sint |= ( HV_SINT_AUTO_EOI |
+		  HV_SINT_VECTOR ( IRQ_INT ( 13 /* See comment above */ ) ) );
+	DBGC2 ( hv, "HV %p SINT%d MSR is %#08llx\n", hv, sintx, sint );
+	wrmsr ( msr, sint );
+}
+
+/**
+ * Disable synthetic interrupt
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v sintx		Synthetic interrupt number
+ */
+void hv_disable_sint ( struct hv_hypervisor *hv, unsigned int sintx ) {
+	unsigned long msr = HV_X64_MSR_SINT ( sintx );
+	uint64_t sint;
+
+	/* Do nothing if interrupt is already disabled */
+	sint = rdmsr ( msr );
+	if ( sint & HV_SINT_MASKED )
+		return;
+
+	/* Disable synthetic interrupt */
+	sint &= ~HV_SINT_AUTO_EOI;
+	sint |= HV_SINT_MASKED;
+	DBGC2 ( hv, "HV %p SINT%d MSR is %#08llx\n", hv, sintx, sint );
+	wrmsr ( msr, sint );
+}
+
+/**
+ * Post message
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v id		Connection ID
+ * @v type		Message type
+ * @v data		Message
+ * @v len		Length of message
+ * @ret rc		Return status code
+ */
+int hv_post_message ( struct hv_hypervisor *hv, unsigned int id,
+		      unsigned int type, const void *data, size_t len ) {
+	struct hv_post_message *msg = &hv->message->posted;
+	int status;
+	int rc;
+
+	/* Sanity check */
+	assert ( len <= sizeof ( msg->data ) );
+
+	/* Construct message */
+	memset ( msg, 0, sizeof ( *msg ) );
+	msg->id = cpu_to_le32 ( id );
+	msg->type = cpu_to_le32 ( type );
+	msg->len = cpu_to_le32 ( len );
+	memcpy ( msg->data, data, len );
+	DBGC2 ( hv, "HV %p connection %d posting message type %#08x:\n",
+		hv, id, type );
+	DBGC2_HDA ( hv, 0, msg->data, len );
+
+	/* Post message */
+	if ( ( status = hv_call ( hv, HV_POST_MESSAGE, msg, NULL ) ) != 0 ) {
+		rc = -EHV ( status );
+		DBGC ( hv, "HV %p could not post message to %#08x: %s\n",
+		       hv, id, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * Wait for received message
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v sintx		Synthetic interrupt number
+ * @ret rc		Return status code
+ */
+int hv_wait_for_message ( struct hv_hypervisor *hv, unsigned int sintx ) {
+	struct hv_message *msg = &hv->message->received;
+	struct hv_message *src = &hv->synic.message[sintx];
+	unsigned int retries;
+	size_t len;
+
+	/* Wait for message to arrive */
+	for ( retries = 0 ; retries < HV_MESSAGE_MAX_WAIT_MS ; retries++ ) {
+
+		/* Check for message */
+		if ( src->type ) {
+
+			/* Copy message */
+			memset ( msg, 0, sizeof ( *msg ) );
+			len = src->len;
+			assert ( len <= sizeof ( *msg ) );
+			memcpy ( msg, src,
+				 ( offsetof ( typeof ( *msg ), data ) + len ) );
+			DBGC2 ( hv, "HV %p SINT%d received message type "
+				"%#08x:\n", hv, sintx,
+				le32_to_cpu ( msg->type ) );
+			DBGC2_HDA ( hv, 0, msg->data, len );
+
+			/* Consume message */
+			src->type = 0;
+
+			return 0;
+		}
+
+		/* Trigger message delivery */
+		wrmsr ( HV_X64_MSR_EOM, 0 );
+
+		/* Delay */
+		mdelay ( 1 );
+	}
+
+	DBGC ( hv, "HV %p SINT%d timed out waiting for message\n",
+	       hv, sintx );
+	return -ETIMEDOUT;
+}
+
+/**
+ * Signal event
+ *
+ * @v hv		Hyper-V hypervisor
+ * @v id		Connection ID
+ * @v flag		Flag number
+ * @ret rc		Return status code
+ */
+int hv_signal_event ( struct hv_hypervisor *hv, unsigned int id,
+		      unsigned int flag ) {
+	struct hv_signal_event *event = &hv->message->signalled;
+	int status;
+	int rc;
+
+	/* Construct event */
+	memset ( event, 0, sizeof ( *event ) );
+	event->id = cpu_to_le32 ( id );
+	event->flag = cpu_to_le16 ( flag );
+
+	/* Signal event */
+	if ( ( status = hv_call ( hv, HV_SIGNAL_EVENT, event, NULL ) ) != 0 ) {
+		rc = -EHV ( status );
+		DBGC ( hv, "HV %p could not signal event to %#08x: %s\n",
+		       hv, id, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * Probe root device
+ *
+ * @v rootdev		Root device
+ * @ret rc		Return status code
+ */
+static int hv_probe ( struct root_device *rootdev ) {
+	struct hv_hypervisor *hv;
+	int rc;
+
+	/* Check we are running in Hyper-V */
+	if ( ( rc = hv_check_hv() ) != 0 )
+		goto err_check_hv;
+
+	/* Allocate and initialise structure */
+	hv = zalloc ( sizeof ( *hv ) );
+	if ( ! hv ) {
+		rc = -ENOMEM;
+		goto err_alloc;
+	}
+
+	/* Check features */
+	if ( ( rc = hv_check_features ( hv ) ) != 0 )
+		goto err_check_features;
+
+	/* Check that Gen 2 UEFI firmware is not running */
+	if ( ( rc = hv_check_uefi ( hv ) ) != 0 )
+		goto err_check_uefi;
+
+	/* Allocate pages */
+	if ( ( rc = hv_alloc_pages ( hv, &hv->hypercall, &hv->synic.message,
+				     &hv->synic.event, NULL ) ) != 0 )
+		goto err_alloc_pages;
+
+	/* Allocate message buffer */
+	if ( ( rc = hv_alloc_message ( hv ) ) != 0 )
+		goto err_alloc_message;
+
+	/* Map hypercall page */
+	hv_map_hypercall ( hv );
+
+	/* Map synthetic interrupt controller */
+	hv_map_synic ( hv );
+
+	/* Probe Hyper-V devices */
+	if ( ( rc = vmbus_probe ( hv, &rootdev->dev ) ) != 0 )
+		goto err_vmbus_probe;
+
+	rootdev_set_drvdata ( rootdev, hv );
+	return 0;
+
+	vmbus_remove ( hv, &rootdev->dev );
+ err_vmbus_probe:
+	hv_unmap_synic ( hv );
+	hv_unmap_hypercall ( hv );
+	hv_free_message ( hv );
+ err_alloc_message:
+	hv_free_pages ( hv, hv->hypercall, hv->synic.message, hv->synic.event,
+			NULL );
+ err_alloc_pages:
+ err_check_uefi:
+ err_check_features:
+	free ( hv );
+ err_alloc:
+ err_check_hv:
+	return rc;
+}
+
+/**
+ * Remove root device
+ *
+ * @v rootdev		Root device
+ */
+static void hv_remove ( struct root_device *rootdev ) {
+	struct hv_hypervisor *hv = rootdev_get_drvdata ( rootdev );
+
+	vmbus_remove ( hv, &rootdev->dev );
+	hv_unmap_synic ( hv );
+	hv_unmap_hypercall ( hv );
+	hv_free_message ( hv );
+	hv_free_pages ( hv, hv->hypercall, hv->synic.message, hv->synic.event,
+			NULL );
+	free ( hv );
+	rootdev_set_drvdata ( rootdev, NULL );
+}
+
+/** Hyper-V root device driver */
+static struct root_driver hv_root_driver = {
+	.probe = hv_probe,
+	.remove = hv_remove,
+};
+
+/** Hyper-V root device */
+struct root_device hv_root_device __root_device = {
+	.dev = { .name = "Hyper-V" },
+	.driver = &hv_root_driver,
+};
+
+/**
+ * Quiesce system
+ *
+ */
+static void hv_quiesce ( void ) {
+	struct hv_hypervisor *hv = rootdev_get_drvdata ( &hv_root_device );
+	unsigned int i;
+
+	/* Do nothing if we are not running in Hyper-V */
+	if ( ! hv )
+		return;
+
+	/* The "enlightened" portions of the Windows Server 2016 boot
+	 * process will not cleanly take ownership of an active
+	 * Hyper-V connection.  Experimentation shows that the minimum
+	 * requirement is that we disable the SynIC message page
+	 * (i.e. zero the SIMP MSR).
+	 *
+	 * We cannot perform a full shutdown of the Hyper-V
+	 * connection.  Experimentation shows that if we disable the
+	 * SynIC (i.e. zero the SCONTROL MSR) then Windows Server 2016
+	 * will enter an indefinite wait loop.
+	 *
+	 * Attempt to create a safe handover environment by resetting
+	 * all MSRs except for SCONTROL.
+	 *
+	 * Note that we do not shut down our VMBus devices, since we
+	 * may need to unquiesce the system and continue operation.
+	 */
+
+	/* Disable all synthetic interrupts */
+	for ( i = 0 ; i <= HV_SINT_MAX ; i++ )
+		hv_disable_sint ( hv, i );
+
+	/* Unmap synthetic interrupt controller, leaving SCONTROL
+	 * enabled (see above).
+	 */
+	hv_unmap_synic_no_scontrol ( hv );
+
+	/* Unmap hypercall page */
+	hv_unmap_hypercall ( hv );
+
+	DBGC ( hv, "HV %p quiesced\n", hv );
+}
+
+/**
+ * Unquiesce system
+ *
+ */
+static void hv_unquiesce ( void ) {
+	struct hv_hypervisor *hv = rootdev_get_drvdata ( &hv_root_device );
+	uint64_t simp;
+	int rc;
+
+	/* Do nothing if we are not running in Hyper-V */
+	if ( ! hv )
+		return;
+
+	/* Experimentation shows that the "enlightened" portions of
+	 * Windows Server 2016 will break our Hyper-V connection at
+	 * some point during a SAN boot.  Surprisingly it does not
+	 * change the guest OS ID MSR, but it does leave the SynIC
+	 * message page disabled.
+	 *
+	 * Our own explicit quiescing procedure will also disable the
+	 * SynIC message page.  We can therefore use the SynIC message
+	 * page enable bit as a heuristic to determine when we need to
+	 * reestablish our Hyper-V connection.
+	 */
+	simp = rdmsr ( HV_X64_MSR_SIMP );
+	if ( simp & HV_SIMP_ENABLE )
+		return;
+
+	/* Remap hypercall page */
+	hv_map_hypercall ( hv );
+
+	/* Remap synthetic interrupt controller */
+	hv_map_synic ( hv );
+
+	/* Reset Hyper-V devices */
+	if ( ( rc = vmbus_reset ( hv, &hv_root_device.dev ) ) != 0 ) {
+		DBGC ( hv, "HV %p could not unquiesce: %s\n",
+		       hv, strerror ( rc ) );
+		/* Nothing we can do */
+		return;
+	}
+}
+
+/** Hyper-V quiescer */
+struct quiescer hv_quiescer __quiescer = {
+	.quiesce = hv_quiesce,
+	.unquiesce = hv_unquiesce,
+};
+
+/**
+ * Probe timer
+ *
+ * @ret rc		Return status code
+ */
+static int hv_timer_probe ( void ) {
+	uint32_t available;
+	uint32_t discard_ebx;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+	int rc;
+
+	/* Check we are running in Hyper-V */
+	if ( ( rc = hv_check_hv() ) != 0 )
+		return rc;
+
+	/* Check for available reference counter */
+	cpuid ( HV_CPUID_FEATURES, 0, &available, &discard_ebx, &discard_ecx,
+		&discard_edx );
+	if ( ! ( available & HV_FEATURES_AVAIL_TIME_REF_COUNT_MSR ) ) {
+		DBGC ( HV_INTERFACE_ID, "HV has no time reference counter\n" );
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * Get current system time in ticks
+ *
+ * @ret ticks		Current time, in ticks
+ */
+static unsigned long hv_currticks ( void ) {
+
+	/* Calculate time using a combination of bit shifts and
+	 * multiplication (to avoid a 64-bit division).
+	 */
+	return ( ( rdmsr ( HV_X64_MSR_TIME_REF_COUNT ) >> HV_TIMER_SHIFT ) *
+		 ( TICKS_PER_SEC / ( HV_TIMER_HZ >> HV_TIMER_SHIFT ) ) );
+}
+
+/**
+ * Delay for a fixed number of microseconds
+ *
+ * @v usecs		Number of microseconds for which to delay
+ */
+static void hv_udelay ( unsigned long usecs ) {
+	uint32_t start;
+	uint32_t elapsed;
+	uint32_t threshold;
+
+	/* Spin until specified number of 10MHz ticks have elapsed */
+	start = rdmsr ( HV_X64_MSR_TIME_REF_COUNT );
+	threshold = ( usecs * ( HV_TIMER_HZ / 1000000 ) );
+	do {
+		elapsed = ( rdmsr ( HV_X64_MSR_TIME_REF_COUNT ) - start );
+	} while ( elapsed < threshold );
+}
+
+/** Hyper-V timer */
+struct timer hv_timer __timer ( TIMER_PREFERRED ) = {
+	.name = "Hyper-V",
+	.probe = hv_timer_probe,
+	.currticks = hv_currticks,
+	.udelay = hv_udelay,
+};
+
+/* Drag in objects via hv_root_device */
+REQUIRING_SYMBOL ( hv_root_device );
+
+/* Drag in netvsc driver */
+//REQUIRE_OBJECT ( netvsc );

IPXE/ipxe-3fe683e/src/arch/x86/drivers/xen/hvm.c

+/*
+ * Copyright (C) 2014 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stdint.h>
+#include <stdio.h>
+#include <errno.h>
+#include <ipxe/malloc.h>
+#include <ipxe/pci.h>
+#include <ipxe/cpuid.h>
+#include <ipxe/msr.h>
+#include <ipxe/xen.h>
+#include <ipxe/xenver.h>
+#include <ipxe/xenmem.h>
+#include <ipxe/xenstore.h>
+#include <ipxe/xenbus.h>
+#include <ipxe/xengrant.h>
+#include "hvm.h"
+
+/** @file
+ *
+ * Xen HVM driver
+ *
+ */
+
+/**
+ * Get CPUID base
+ *
+ * @v hvm		HVM device
+ * @ret rc		Return status code
+ */
+static int hvm_cpuid_base ( struct hvm_device *hvm ) {
+	struct {
+		uint32_t ebx;
+		uint32_t ecx;
+		uint32_t edx;
+	} __attribute__ (( packed )) signature;
+	uint32_t base;
+	uint32_t version;
+	uint32_t discard_eax;
+	uint32_t discard_ebx;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+
+	/* Scan for magic signature */
+	for ( base = HVM_CPUID_MIN ; base <= HVM_CPUID_MAX ;
+	      base += HVM_CPUID_STEP ) {
+		cpuid ( base, 0, &discard_eax, &signature.ebx, &signature.ecx,
+			&signature.edx );
+		if ( memcmp ( &signature, HVM_CPUID_MAGIC,
+			      sizeof ( signature ) ) == 0 ) {
+			hvm->cpuid_base = base;
+			cpuid ( ( base + HVM_CPUID_VERSION ), 0, &version,
+				&discard_ebx, &discard_ecx, &discard_edx );
+			DBGC2 ( hvm, "HVM using CPUID base %#08x (v%d.%d)\n",
+				base, ( version >> 16 ), ( version & 0xffff ) );
+			return 0;
+		}
+	}
+
+	DBGC ( hvm, "HVM could not find hypervisor\n" );
+	return -ENODEV;
+}
+
+/**
+ * Map hypercall page(s)
+ *
+ * @v hvm		HVM device
+ * @ret rc		Return status code
+ */
+static int hvm_map_hypercall ( struct hvm_device *hvm ) {
+	uint32_t pages;
+	uint32_t msr;
+	uint32_t discard_ecx;
+	uint32_t discard_edx;
+	physaddr_t hypercall_phys;
+	uint32_t version;
+	static xen_extraversion_t extraversion;
+	int xenrc;
+	int rc;
+
+	/* Get number of hypercall pages and MSR to use */
+	cpuid ( ( hvm->cpuid_base + HVM_CPUID_PAGES ), 0, &pages, &msr,
+		&discard_ecx, &discard_edx );
+
+	/* Allocate pages */
+	hvm->hypercall_len = ( pages * PAGE_SIZE );
+	hvm->xen.hypercall = malloc_dma ( hvm->hypercall_len, PAGE_SIZE );
+	if ( ! hvm->xen.hypercall ) {
+		DBGC ( hvm, "HVM could not allocate %d hypercall page(s)\n",
+		       pages );
+		return -ENOMEM;
+	}
+	hypercall_phys = virt_to_phys ( hvm->xen.hypercall );
+	DBGC2 ( hvm, "HVM hypercall page(s) at [%#08lx,%#08lx) via MSR %#08x\n",
+		hypercall_phys, ( hypercall_phys + hvm->hypercall_len ), msr );
+
+	/* Write to MSR */
+	wrmsr ( msr, hypercall_phys );
+
+	/* Check that hypercall mechanism is working */
+	version = xenver_version ( &hvm->xen );
+	if ( ( xenrc = xenver_extraversion ( &hvm->xen, &extraversion ) ) != 0){
+		rc = -EXEN ( xenrc );
+		DBGC ( hvm, "HVM could not get extraversion: %s\n",
+		       strerror ( rc ) );
+		return rc;
+	}
+	DBGC2 ( hvm, "HVM found Xen version %d.%d%s\n",
+		( version >> 16 ), ( version & 0xffff ) , extraversion );
+
+	return 0;
+}
+
+/**
+ * Unmap hypercall page(s)
+ *
+ * @v hvm		HVM device
+ */
+static void hvm_unmap_hypercall ( struct hvm_device *hvm ) {
+
+	/* Free pages */
+	free_dma ( hvm->xen.hypercall, hvm->hypercall_len );
+}
+
+/**
+ * Allocate and map MMIO space
+ *
+ * @v hvm		HVM device
+ * @v space		Source mapping space
+ * @v len		Length (must be a multiple of PAGE_SIZE)
+ * @ret mmio		MMIO space address, or NULL on error
+ */
+static void * hvm_ioremap ( struct hvm_device *hvm, unsigned int space,
+			    size_t len ) {
+	struct xen_add_to_physmap add;
+	struct xen_remove_from_physmap remove;
+	unsigned int pages = ( len / PAGE_SIZE );
+	physaddr_t mmio_phys;
+	unsigned int i;
+	void *mmio;
+	int xenrc;
+	int rc;
+
+	/* Sanity check */
+	assert ( ( len % PAGE_SIZE ) == 0 );
+
+	/* Check for available space */
+	if ( ( hvm->mmio_offset + len ) > hvm->mmio_len ) {
+		DBGC ( hvm, "HVM could not allocate %zd bytes of MMIO space "
+		       "(%zd of %zd remaining)\n", len,
+		       ( hvm->mmio_len - hvm->mmio_offset ), hvm->mmio_len );
+		goto err_no_space;
+	}
+
+	/* Map this space */
+	mmio = ioremap ( ( hvm->mmio + hvm->mmio_offset ), len );
+	if ( ! mmio ) {
+		DBGC ( hvm, "HVM could not map MMIO space [%08lx,%08lx)\n",
+		       ( hvm->mmio + hvm->mmio_offset ),
+		       ( hvm->mmio + hvm->mmio_offset + len ) );
+		goto err_ioremap;
+	}
+	mmio_phys = virt_to_phys ( mmio );
+
+	/* Add to physical address space */
+	for ( i = 0 ; i < pages ; i++ ) {
+		add.domid = DOMID_SELF;
+		add.idx = i;
+		add.space = space;
+		add.gpfn = ( ( mmio_phys / PAGE_SIZE ) + i );
+		if ( ( xenrc = xenmem_add_to_physmap ( &hvm->xen, &add ) ) !=0){
+			rc = -EXEN ( xenrc );
+			DBGC ( hvm, "HVM could not add space %d idx %d at "
+			       "[%08lx,%08lx): %s\n", space, i,
+			       ( mmio_phys + ( i * PAGE_SIZE ) ),
+			       ( mmio_phys + ( ( i + 1 ) * PAGE_SIZE ) ),
+			       strerror ( rc ) );
+			goto err_add_to_physmap;
+		}
+	}
+
+	/* Update offset */
+	hvm->mmio_offset += len;
+
+	return mmio;
+
+	i = pages;
+ err_add_to_physmap:
+	for ( i-- ; ( signed int ) i >= 0 ; i-- ) {
+		remove.domid = DOMID_SELF;
+		add.gpfn = ( ( mmio_phys / PAGE_SIZE ) + i );
+		xenmem_remove_from_physmap ( &hvm->xen, &remove );
+	}
+	iounmap ( mmio );
+ err_ioremap:
+ err_no_space:
+	return NULL;
+}
+
+/**
+ * Unmap MMIO space
+ *
+ * @v hvm		HVM device
+ * @v mmio		MMIO space address
+ * @v len		Length (must be a multiple of PAGE_SIZE)
+ */
+static void hvm_iounmap ( struct hvm_device *hvm, void *mmio, size_t len ) {
+	struct xen_remove_from_physmap remove;
+	physaddr_t mmio_phys = virt_to_phys ( mmio );
+	unsigned int pages = ( len / PAGE_SIZE );
+	unsigned int i;
+	int xenrc;
+	int rc;
+
+	/* Unmap this space */
+	iounmap ( mmio );
+
+	/* Remove from physical address space */
+	for ( i = 0 ; i < pages ; i++ ) {
+		remove.domid = DOMID_SELF;
+		remove.gpfn = ( ( mmio_phys / PAGE_SIZE ) + i );
+		if ( ( xenrc = xenmem_remove_from_physmap ( &hvm->xen,
+							    &remove ) ) != 0 ) {
+			rc = -EXEN ( xenrc );
+			DBGC ( hvm, "HVM could not remove space [%08lx,%08lx): "
+			       "%s\n", ( mmio_phys + ( i * PAGE_SIZE ) ),
+			       ( mmio_phys + ( ( i + 1 ) * PAGE_SIZE ) ),
+			       strerror ( rc ) );
+			/* Nothing we can do about this */
+		}
+	}
+}
+
+/**
+ * Map shared info page
+ *
+ * @v hvm		HVM device
+ * @ret rc		Return status code
+ */
+static int hvm_map_shared_info ( struct hvm_device *hvm ) {
+	physaddr_t shared_info_phys;
+	int rc;
+
+	/* Map shared info page */
+	hvm->xen.shared = hvm_ioremap ( hvm, XENMAPSPACE_shared_info,
+					PAGE_SIZE );
+	if ( ! hvm->xen.shared ) {
+		rc = -ENOMEM;
+		goto err_alloc;
+	}
+	shared_info_phys = virt_to_phys ( hvm->xen.shared );
+	DBGC2 ( hvm, "HVM shared info page at [%#08lx,%#08lx)\n",
+		shared_info_phys, ( shared_info_phys + PAGE_SIZE ) );
+
+	/* Sanity check */
+	DBGC2 ( hvm, "HVM wallclock time is %d\n",
+		readl ( &hvm->xen.shared->wc_sec ) );
+
+	return 0;
+
+	hvm_iounmap ( hvm, hvm->xen.shared, PAGE_SIZE );
+ err_alloc:
+	return rc;
+}
+
+/**
+ * Unmap shared info page
+ *
+ * @v hvm		HVM device
+ */
+static void hvm_unmap_shared_info ( struct hvm_device *hvm ) {
+
+	/* Unmap shared info page */
+	hvm_iounmap ( hvm, hvm->xen.shared, PAGE_SIZE );
+}
+
+/**
+ * Map grant table
+ *
+ * @v hvm		HVM device
+ * @ret rc		Return status code
+ */
+static int hvm_map_grant ( struct hvm_device *hvm ) {
+	physaddr_t grant_phys;
+	int rc;
+
+	/* Initialise grant table */
+	if ( ( rc = xengrant_init ( &hvm->xen ) ) != 0 ) {
+		DBGC ( hvm, "HVM could not initialise grant table: %s\n",
+		       strerror ( rc ) );
+		return rc;
+	}
+
+	/* Map grant table */
+	hvm->xen.grant.table = hvm_ioremap ( hvm, XENMAPSPACE_grant_table,
+					     hvm->xen.grant.len );
+	if ( ! hvm->xen.grant.table )
+		return -ENODEV;
+
+	grant_phys = virt_to_phys ( hvm->xen.grant.table );
+	DBGC2 ( hvm, "HVM mapped grant table at [%08lx,%08lx)\n",
+		grant_phys, ( grant_phys + hvm->xen.grant.len ) );
+	return 0;
+}
+
+/**
+ * Unmap grant table
+ *
+ * @v hvm		HVM device
+ */
+static void hvm_unmap_grant ( struct hvm_device *hvm ) {
+
+	/* Unmap grant table */
+	hvm_iounmap ( hvm, hvm->xen.grant.table, hvm->xen.grant.len );
+}
+
+/**
+ * Map XenStore
+ *
+ * @v hvm		HVM device
+ * @ret rc		Return status code
+ */
+static int hvm_map_xenstore ( struct hvm_device *hvm ) {
+	uint64_t xenstore_evtchn;
+	uint64_t xenstore_pfn;
+	physaddr_t xenstore_phys;
+	char *name;
+	int xenrc;
+	int rc;
+
+	/* Get XenStore event channel */
+	if ( ( xenrc = xen_hvm_get_param ( &hvm->xen, HVM_PARAM_STORE_EVTCHN,
+					   &xenstore_evtchn ) ) != 0 ) {
+		rc = -EXEN ( xenrc );
+		DBGC ( hvm, "HVM could not get XenStore event channel: %s\n",
+		       strerror ( rc ) );
+		return rc;
+	}
+	hvm->xen.store.port = xenstore_evtchn;
+
+	/* Get XenStore PFN */
+	if ( ( xenrc = xen_hvm_get_param ( &hvm->xen, HVM_PARAM_STORE_PFN,
+					   &xenstore_pfn ) ) != 0 ) {
+		rc = -EXEN ( xenrc );
+		DBGC ( hvm, "HVM could not get XenStore PFN: %s\n",
+		       strerror ( rc ) );
+		return rc;
+	}
+	xenstore_phys = ( xenstore_pfn * PAGE_SIZE );
+
+	/* Map XenStore */
+	hvm->xen.store.intf = ioremap ( xenstore_phys, PAGE_SIZE );
+	if ( ! hvm->xen.store.intf ) {
+		DBGC ( hvm, "HVM could not map XenStore at [%08lx,%08lx)\n",
+		       xenstore_phys, ( xenstore_phys + PAGE_SIZE ) );
+		return -ENODEV;
+	}
+	DBGC2 ( hvm, "HVM mapped XenStore at [%08lx,%08lx) with event port "
+		"%d\n", xenstore_phys, ( xenstore_phys + PAGE_SIZE ),
+		hvm->xen.store.port );
+
+	/* Check that XenStore is working */
+	if ( ( rc = xenstore_read ( &hvm->xen, &name, "name", NULL ) ) != 0 ) {
+		DBGC ( hvm, "HVM could not read domain name: %s\n",
+		       strerror ( rc ) );
+		return rc;
+	}
+	DBGC2 ( hvm, "HVM running in domain \"%s\"\n", name );
+	free ( name );
+
+	return 0;
+}
+
+/**
+ * Unmap XenStore
+ *
+ * @v hvm		HVM device
+ */
+static void hvm_unmap_xenstore ( struct hvm_device *hvm ) {
+
+	/* Unmap XenStore */
+	iounmap ( hvm->xen.store.intf );
+}
+
+/**
+ * Probe PCI device
+ *
+ * @v pci		PCI device
+ * @ret rc		Return status code
+ */
+static int hvm_probe ( struct pci_device *pci ) {
+	struct hvm_device *hvm;
+	int rc;
+
+	/* Allocate and initialise structure */
+	hvm = zalloc ( sizeof ( *hvm ) );
+	if ( ! hvm ) {
+		rc = -ENOMEM;
+		goto err_alloc;
+	}
+	hvm->mmio = pci_bar_start ( pci, HVM_MMIO_BAR );
+	hvm->mmio_len = pci_bar_size ( pci, HVM_MMIO_BAR );
+	DBGC2 ( hvm, "HVM has MMIO space [%08lx,%08lx)\n",
+		hvm->mmio, ( hvm->mmio + hvm->mmio_len ) );
+
+	/* Fix up PCI device */
+	adjust_pci_device ( pci );
+
+	/* Attach to hypervisor */
+	if ( ( rc = hvm_cpuid_base ( hvm ) ) != 0 )
+		goto err_cpuid_base;
+	if ( ( rc = hvm_map_hypercall ( hvm ) ) != 0 )
+		goto err_map_hypercall;
+	if ( ( rc = hvm_map_shared_info ( hvm ) ) != 0 )
+		goto err_map_shared_info;
+	if ( ( rc = hvm_map_grant ( hvm ) ) != 0 )
+		goto err_map_grant;
+	if ( ( rc = hvm_map_xenstore ( hvm ) ) != 0 )
+		goto err_map_xenstore;
+
+	/* Probe Xen devices */
+	if ( ( rc = xenbus_probe ( &hvm->xen, &pci->dev ) ) != 0 ) {
+		DBGC ( hvm, "HVM could not probe Xen bus: %s\n",
+		       strerror ( rc ) );
+		goto err_xenbus_probe;
+	}
+
+	pci_set_drvdata ( pci, hvm );
+	return 0;
+
+	xenbus_remove ( &hvm->xen, &pci->dev );
+ err_xenbus_probe:
+	hvm_unmap_xenstore ( hvm );
+ err_map_xenstore:
+	hvm_unmap_grant ( hvm );
+ err_map_grant:
+	hvm_unmap_shared_info ( hvm );
+ err_map_shared_info:
+	hvm_unmap_hypercall ( hvm );
+ err_map_hypercall:
+ err_cpuid_base:
+	free ( hvm );
+ err_alloc:
+	return rc;
+}
+
+/**
+ * Remove PCI device
+ *
+ * @v pci		PCI device
+ */
+static void hvm_remove ( struct pci_device *pci ) {
+	struct hvm_device *hvm = pci_get_drvdata ( pci );
+
+	xenbus_remove ( &hvm->xen, &pci->dev );
+	hvm_unmap_xenstore ( hvm );
+	hvm_unmap_grant ( hvm );
+	hvm_unmap_shared_info ( hvm );
+	hvm_unmap_hypercall ( hvm );
+	free ( hvm );
+}
+
+/** PCI device IDs */
+static struct pci_device_id hvm_ids[] = {
+	PCI_ROM ( 0x5853, 0x0001, "hvm", "hvm", 0 ),
+	PCI_ROM ( 0x5853, 0x0002, "hvm2", "hvm2", 0 ),
+};
+
+/** PCI driver */
+struct pci_driver hvm_driver __pci_driver = {
+	.ids = hvm_ids,
+	.id_count = ( sizeof ( hvm_ids ) / sizeof ( hvm_ids[0] ) ),
+	.probe = hvm_probe,
+	.remove = hvm_remove,
+};
+
+/* Drag in objects via hvm_driver */
+REQUIRING_SYMBOL ( hvm_driver );
+
+/* Drag in netfront driver */
+//REQUIRE_OBJECT ( netfront );

IPXE/ipxe-3fe683e/src/arch/x86/interface/pcbios/hidemem.c

+/* Copyright (C) 2006 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <assert.h>
+#include <realmode.h>
+#include <biosint.h>
+#include <basemem.h>
+#include <fakee820.h>
+#include <ipxe/init.h>
+#include <ipxe/io.h>
+#include <ipxe/hidemem.h>
+
+/** Set to true if you want to test a fake E820 map */
+#define FAKE_E820 0
+
+/** Alignment for hidden memory regions */
+#define ALIGN_HIDDEN 4096   /* 4kB page alignment should be enough */
+
+/**
+ * A hidden region of iPXE
+ *
+ * This represents a region that will be edited out of the system's
+ * memory map.
+ *
+ * This structure is accessed by assembly code, so must not be
+ * changed.
+ */
+struct hidden_region {
+	/** Physical start address */
+	uint64_t start;
+	/** Physical end address */
+	uint64_t end;
+};
+
+/** Hidden base memory */
+extern struct hidden_region __data16 ( hidemem_base );
+#define hidemem_base __use_data16 ( hidemem_base )
+
+/** Hidden umalloc memory */
+extern struct hidden_region __data16 ( hidemem_umalloc );
+#define hidemem_umalloc __use_data16 ( hidemem_umalloc )
+
+/** Hidden text memory */
+extern struct hidden_region __data16 ( hidemem_textdata );
+#define hidemem_textdata __use_data16 ( hidemem_textdata )
+
+/** Assembly routine in e820mangler.S */
+extern void int15();
+
+/** Vector for storing original INT 15 handler */
+extern struct segoff __text16 ( int15_vector );
+#define int15_vector __use_text16 ( int15_vector )
+
+/* The linker defines these symbols for us */
+extern char _textdata[];
+extern char _etextdata[];
+extern char _text16_memsz[];
+#define _text16_memsz ( ( size_t ) _text16_memsz )
+extern char _data16_memsz[];
+#define _data16_memsz ( ( size_t ) _data16_memsz )
+
+/**
+ * Hide region of memory from system memory map
+ *
+ * @v region		Hidden memory region
+ * @v start		Start of region
+ * @v end		End of region
+ */
+static void hide_region ( struct hidden_region *region,
+			  physaddr_t start, physaddr_t end ) {
+
+	/* Some operating systems get a nasty shock if a region of the
+	 * E820 map seems to start on a non-page boundary.  Make life
+	 * safer by rounding out our edited region.
+	 */
+	region->start = ( start & ~( ALIGN_HIDDEN - 1 ) );
+	region->end = ( ( end + ALIGN_HIDDEN - 1 ) & ~( ALIGN_HIDDEN - 1 ) );
+
+	DBG ( "Hiding region [%llx,%llx)\n", region->start, region->end );
+}
+
+/**
+ * Hide used base memory
+ *
+ */
+void hide_basemem ( void ) {
+	/* Hide from the top of free base memory to 640kB.  Don't use
+	 * hide_region(), because we don't want this rounded to the
+	 * nearest page boundary.
+	 */
+	hidemem_base.start = ( get_fbms() * 1024 );
+}
+
+/**
+ * Hide umalloc() region
+ *
+ */
+void hide_umalloc ( physaddr_t start, physaddr_t end ) {
+	assert ( end <= virt_to_phys ( _textdata ) );
+	hide_region ( &hidemem_umalloc, start, end );
+}
+
+/**
+ * Hide .text and .data
+ *
+ */
+void hide_textdata ( void ) {
+/* Deleted by longpanda */
+#if 0
+	hide_region ( &hidemem_textdata, virt_to_phys ( _textdata ),
+		      virt_to_phys ( _etextdata ) );
+#endif
+}
+
+/**
+ * Hide Etherboot
+ *
+ * Installs an INT 15 handler to edit Etherboot out of the memory map
+ * returned by the BIOS.
+ */
+static void hide_etherboot ( void ) {
+	struct memory_map memmap;
+	unsigned int rm_ds_top;
+	unsigned int rm_cs_top;
+	unsigned int fbms;
+
+	/* Dump memory map before mangling */
+	DBG ( "Hiding iPXE from system memory map\n" );
+	get_memmap ( &memmap );
+
+	/* Hook in fake E820 map, if we're testing one */
+	if ( FAKE_E820 ) {
+		DBG ( "Hooking in fake E820 map\n" );
+		fake_e820();
+		get_memmap ( &memmap );
+	}
+
+	/* Initialise the hidden regions */
+	hide_basemem();
+	hide_umalloc ( virt_to_phys ( _textdata ), virt_to_phys ( _textdata ) );
+	hide_textdata();
+
+	/* Some really moronic BIOSes bring up the PXE stack via the
+	 * UNDI loader entry point and then don't bother to unload it
+	 * before overwriting the code and data segments.  If this
+	 * happens, we really don't want to leave INT 15 hooked,
+	 * because that will cause any loaded OS to die horribly as
+	 * soon as it attempts to fetch the system memory map.
+	 *
+	 * We use a heuristic to guess whether or not we are being
+	 * loaded sensibly.
+	 */
+	rm_cs_top = ( ( ( rm_cs << 4 ) + _text16_memsz + 1024 - 1 ) >> 10 );
+	rm_ds_top = ( ( ( rm_ds << 4 ) + _data16_memsz + 1024 - 1 ) >> 10 );
+	fbms = get_fbms();
+	if ( ( rm_cs_top < fbms ) && ( rm_ds_top < fbms ) ) {
+		DBG ( "Detected potentially unsafe UNDI load at CS=%04x "
+		      "DS=%04x FBMS=%dkB\n", rm_cs, rm_ds, fbms );
+		DBG ( "Disabling INT 15 memory hiding\n" );
+		return;
+	}
+
+	/* Hook INT 15 */
+	hook_bios_interrupt ( 0x15, ( intptr_t ) int15, &int15_vector );
+
+	/* Dump memory map after mangling */
+	DBG ( "Hidden iPXE from system memory map\n" );
+	get_memmap ( &memmap );
+}
+
+/**
+ * Unhide Etherboot
+ *
+ * Uninstalls the INT 15 handler installed by hide_etherboot(), if
+ * possible.
+ */
+static void unhide_etherboot ( int flags __unused ) {
+	struct memory_map memmap;
+	int rc;
+
+	/* If we have more than one hooked interrupt at this point, it
+	 * means that some other vector is still hooked, in which case
+	 * we can't safely unhook INT 15 because we need to keep our
+	 * memory protected.  (We expect there to be at least one
+	 * hooked interrupt, because INT 15 itself is still hooked).
+	 */
+	if ( hooked_bios_interrupts > 1 ) {
+		DBG ( "Cannot unhide: %d interrupt vectors still hooked\n",
+		      hooked_bios_interrupts );
+		return;
+	}
+
+	/* Try to unhook INT 15 */
+	if ( ( rc = unhook_bios_interrupt ( 0x15, ( intptr_t ) int15,
+					    &int15_vector ) ) != 0 ) {
+		DBG ( "Cannot unhook INT15: %s\n", strerror ( rc ) );
+		/* Leave it hooked; there's nothing else we can do,
+		 * and it should be intrinsically safe (though
+		 * wasteful of RAM).
+		 */
+	}
+
+	/* Unhook fake E820 map, if used */
+	if ( FAKE_E820 )
+		unfake_e820();
+
+	/* Dump memory map after unhiding */
+	DBG ( "Unhidden iPXE from system memory map\n" );
+	get_memmap ( &memmap );
+}
+
+/** Hide Etherboot startup function */
+struct startup_fn hide_etherboot_startup_fn __startup_fn ( STARTUP_EARLY ) = {
+	.name = "hidemem",
+	.startup = hide_etherboot,
+	.shutdown = unhide_etherboot,
+};

IPXE/ipxe-3fe683e/src/arch/x86/interface/pcbios/int13.c

+/*
+ * Copyright (C) 2010 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <errno.h>
+#include <ipxe/sanboot.h>
+
+static int null_san_hook ( unsigned int drive __unused,
+			   struct uri **uris __unused,
+			   unsigned int count __unused,
+			   unsigned int flags __unused ) {
+	return -EOPNOTSUPP;
+}
+
+static void null_san_unhook ( unsigned int drive __unused ) {
+	/* Do nothing */
+}
+
+static int null_san_boot ( unsigned int drive __unused,
+			   const char *filename __unused ) {
+	return -EOPNOTSUPP;
+}
+
+static int null_san_describe ( void ) {
+	return -EOPNOTSUPP;
+}
+
+PROVIDE_SANBOOT ( pcbios, san_hook, null_san_hook );
+PROVIDE_SANBOOT ( pcbios, san_unhook, null_san_unhook );
+PROVIDE_SANBOOT ( pcbios, san_boot, null_san_boot );
+PROVIDE_SANBOOT ( pcbios, san_describe, null_san_describe );
+

IPXE/ipxe-3fe683e/src/arch/x86/interface/pcbios/ventoy_int13.c

+/*
+ * Copyright (C) 2006 Michael Brown <mbrown@fensystems.co.uk>.
+ * Copyright (C) 2020 longpanda <admin@ventoy.net>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <limits.h>
+#include <byteswap.h>
+#include <errno.h>
+#include <assert.h>
+#include <ipxe/blockdev.h>
+#include <ipxe/io.h>
+#include <ipxe/acpi.h>
+#include <ipxe/sanboot.h>
+#include <ipxe/device.h>
+#include <ipxe/pci.h>
+#include <ipxe/timer.h>
+#include <ipxe/eltorito.h>
+#include <ipxe/umalloc.h>
+#include <ipxe/acpi.h>
+#include <ipxe/ibft.h>
+#include <realmode.h>
+#include <bios.h>
+#include <biosint.h>
+#include <bootsector.h>
+#include <int13.h>
+#include <ventoy.h>
+#include "ventoy_int13.h"
+
+static unsigned int g_drive_map1 = 0;
+static unsigned int g_drive_map2 = 0;
+
+/** @file
+ *
+ * INT 13 emulation
+ *
+ * This module provides a mechanism for exporting block devices via
+ * the BIOS INT 13 disk interrupt interface.  
+ *
+ */
+
+/** INT 13 SAN device private data */
+struct int13_data {
+	/** BIOS natural drive number (0x00-0xff)
+	 *
+	 * This is the drive number that would have been assigned by
+	 * 'naturally' appending the drive to the end of the BIOS
+	 * drive list.
+	 *
+	 * If the emulated drive replaces a preexisting drive, this is
+	 * the drive number that the preexisting drive gets remapped
+	 * to.
+	 */
+	unsigned int natural_drive;
+
+	/** Number of cylinders
+	 *
+	 * The cylinder number field in an INT 13 call is ten bits
+	 * wide, giving a maximum of 1024 cylinders.  Conventionally,
+	 * when the 7.8GB limit of a CHS address is exceeded, it is
+	 * the number of cylinders that is increased beyond the
+	 * addressable limit.
+	 */
+	unsigned int cylinders;
+	/** Number of heads
+	 *
+	 * The head number field in an INT 13 call is eight bits wide,
+	 * giving a maximum of 256 heads.  However, apparently all
+	 * versions of MS-DOS up to and including Win95 fail with 256
+	 * heads, so the maximum encountered in practice is 255.
+	 */
+	unsigned int heads;
+	/** Number of sectors per track
+	 *
+	 * The sector number field in an INT 13 call is six bits wide,
+	 * giving a maximum of 63 sectors, since sector numbering
+	 * (unlike head and cylinder numbering) starts at 1, not 0.
+	 */
+	unsigned int sectors_per_track;
+
+	/** Address of El Torito boot catalog (if any) */
+	unsigned int boot_catalog;
+	/** Status of last operation */
+	int last_status;
+};
+
+/** Vector for chaining to other INT 13 handlers */
+static struct segoff __text16 ( int13_vector );
+#define int13_vector __use_text16 ( int13_vector )
+
+/** Assembly wrapper */
+extern void int13_wrapper ( void );
+
+/** Dummy floppy disk parameter table */
+static struct int13_fdd_parameters __data16 ( int13_fdd_params ) = {
+	/* 512 bytes per sector */
+	.bytes_per_sector = 0x02,
+	/* Highest sectors per track that we ever return */
+	.sectors_per_track = 48,
+};
+#define int13_fdd_params __use_data16 ( int13_fdd_params )
+
+/**
+ * Equipment word
+ *
+ * This is a cached copy of the BIOS Data Area equipment word at
+ * 40:10.
+ */
+static uint16_t equipment_word;
+
+/**
+ * Number of BIOS floppy disk drives
+ *
+ * This is derived from the equipment word.  It is held in .text16 to
+ * allow for easy access by the INT 13,08 wrapper.
+ */
+static uint8_t __text16 ( num_fdds );
+#define num_fdds __use_text16 ( num_fdds )
+
+/**
+ * Number of BIOS hard disk drives
+ *
+ * This is a cached copy of the BIOS Data Area number of hard disk
+ * drives at 40:75.  It is held in .text16 to allow for easy access by
+ * the INT 13,08 wrapper.
+ */
+static uint8_t __text16 ( num_drives );
+#define num_drives __use_text16 ( num_drives )
+static struct san_device *g_sandev;
+
+/**
+ * Calculate SAN device capacity (limited to 32 bits)
+ *
+ * @v sandev		SAN device
+ * @ret blocks		Number of blocks
+ */
+static inline uint32_t int13_capacity32 ( struct san_device *sandev ) {
+	uint64_t capacity = sandev_capacity ( sandev );
+	return ( ( capacity <= 0xffffffffUL ) ? capacity : 0xffffffff );
+}
+
+/**
+ * Test if SAN device is a floppy disk drive
+ *
+ * @v sandev		SAN device
+ * @ret is_fdd		SAN device is a floppy disk drive
+ */
+static inline int int13_is_fdd ( struct san_device *sandev ) {
+    (void)sandev;
+	return 0;
+}
+
+#if 0
+/**
+ * Guess INT 13 hard disk drive geometry
+ *
+ * @v sandev		SAN device
+ * @v scratch		Scratch area for single-sector reads
+ * @ret heads		Guessed number of heads
+ * @ret sectors		Guessed number of sectors per track
+ * @ret rc		Return status code
+ *
+ * Guesses the drive geometry by inspecting the partition table.
+ */
+static int int13_guess_geometry_hdd ( struct san_device *sandev, void *scratch,
+				      unsigned int *heads,
+				      unsigned int *sectors ) {
+	struct master_boot_record *mbr = scratch;
+	struct partition_table_entry *partition;
+	unsigned int i;
+	unsigned int start_cylinder;
+	unsigned int start_head;
+	unsigned int start_sector;
+	unsigned int end_head;
+	unsigned int end_sector;
+	int rc;
+
+	/* Read partition table */
+	if ( ( rc = sandev_read ( sandev, 0, 1, virt_to_user ( mbr ) ) ) != 0 ) {
+		DBGC ( sandev, "INT13 drive %02x could not read "
+		       "partition table to guess geometry: %s\n",
+		       sandev->drive, strerror ( rc ) );
+		return rc;
+	}
+	DBGC2 ( sandev, "INT13 drive %02x has MBR:\n", sandev->drive );
+	DBGC2_HDA ( sandev, 0, mbr, sizeof ( *mbr ) );
+	DBGC ( sandev, "INT13 drive %02x has signature %08x\n",
+	       sandev->drive, mbr->signature );
+
+	/* Scan through partition table and modify guesses for
+	 * heads and sectors_per_track if we find any used
+	 * partitions.
+	 */
+	*heads = 0;
+	*sectors = 0;
+	for ( i = 0 ; i < 4 ; i++ ) {
+
+		/* Skip empty partitions */
+		partition = &mbr->partitions[i];
+		if ( ! partition->type )
+			continue;
+
+		/* If partition starts on cylinder 0 then we can
+		 * unambiguously determine the number of sectors.
+		 */
+		start_cylinder = PART_CYLINDER ( partition->chs_start );
+		start_head = PART_HEAD ( partition->chs_start );
+		start_sector = PART_SECTOR ( partition->chs_start );
+		if ( ( start_cylinder == 0 ) && ( start_head != 0 ) ) {
+			*sectors = ( ( partition->start + 1 - start_sector ) /
+				     start_head );
+			DBGC ( sandev, "INT13 drive %02x guessing C/H/S "
+			       "xx/xx/%d based on partition %d\n",
+			       sandev->drive, *sectors, ( i + 1 ) );
+		}
+
+		/* If partition ends on a higher head or sector number
+		 * than our current guess, then increase the guess.
+		 */
+		end_head = PART_HEAD ( partition->chs_end );
+		end_sector = PART_SECTOR ( partition->chs_end );
+		if ( ( end_head + 1 ) > *heads ) {
+			*heads = ( end_head + 1 );
+			DBGC ( sandev, "INT13 drive %02x guessing C/H/S "
+			       "xx/%d/xx based on partition %d\n",
+			       sandev->drive, *heads, ( i + 1 ) );
+		}
+		if ( end_sector > *sectors ) {
+			*sectors = end_sector;
+			DBGC ( sandev, "INT13 drive %02x guessing C/H/S "
+			       "xx/xx/%d based on partition %d\n",
+			       sandev->drive, *sectors, ( i + 1 ) );
+		}
+	}
+
+	/* Default guess is xx/255/63 */
+	if ( ! *heads )
+		*heads = 255;
+	if ( ! *sectors )
+		*sectors = 63;
+
+	return 0;
+}
+
+/** Recognised floppy disk geometries */
+static const struct int13_fdd_geometry int13_fdd_geometries[] = {
+	INT13_FDD_GEOMETRY ( 40, 1, 8 ),
+	INT13_FDD_GEOMETRY ( 40, 1, 9 ),
+	INT13_FDD_GEOMETRY ( 40, 2, 8 ),
+	INT13_FDD_GEOMETRY ( 40, 1, 9 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 8 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 9 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 15 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 18 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 20 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 21 ),
+	INT13_FDD_GEOMETRY ( 82, 2, 21 ),
+	INT13_FDD_GEOMETRY ( 83, 2, 21 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 22 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 23 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 24 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 36 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 39 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 40 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 44 ),
+	INT13_FDD_GEOMETRY ( 80, 2, 48 ),
+};
+
+/**
+ * Guess INT 13 floppy disk drive geometry
+ *
+ * @v sandev		SAN device
+ * @ret heads		Guessed number of heads
+ * @ret sectors		Guessed number of sectors per track
+ * @ret rc		Return status code
+ *
+ * Guesses the drive geometry by inspecting the disk size.
+ */
+static int int13_guess_geometry_fdd ( struct san_device *sandev,
+				      unsigned int *heads,
+				      unsigned int *sectors ) {
+	unsigned int blocks = sandev_capacity ( sandev );
+	const struct int13_fdd_geometry *geometry;
+	unsigned int cylinders;
+	unsigned int i;
+
+	/* Look for a match against a known geometry */
+	for ( i = 0 ; i < ( sizeof ( int13_fdd_geometries ) /
+			    sizeof ( int13_fdd_geometries[0] ) ) ; i++ ) {
+		geometry = &int13_fdd_geometries[i];
+		cylinders = INT13_FDD_CYLINDERS ( geometry );
+		*heads = INT13_FDD_HEADS ( geometry );
+		*sectors = INT13_FDD_SECTORS ( geometry );
+		if ( ( cylinders * (*heads) * (*sectors) ) == blocks ) {
+			DBGC ( sandev, "INT13 drive %02x guessing C/H/S "
+			       "%d/%d/%d based on size %dK\n", sandev->drive,
+			       cylinders, *heads, *sectors, ( blocks / 2 ) );
+			return 0;
+		}
+	}
+
+	/* Otherwise, assume a partial disk image in the most common
+	 * format (1440K, 80/2/18).
+	 */
+	*heads = 2;
+	*sectors = 18;
+	DBGC ( sandev, "INT13 drive %02x guessing C/H/S xx/%d/%d based on size "
+	       "%dK\n", sandev->drive, *heads, *sectors, ( blocks / 2 ) );
+	return 0;
+}
+
+/**
+ * Guess INT 13 drive geometry
+ *
+ * @v sandev		SAN device
+ * @v scratch		Scratch area for single-sector reads
+ * @ret rc		Return status code
+ */
+static int int13_guess_geometry ( struct san_device *sandev, void *scratch ) {
+	struct int13_data *int13 = sandev->priv;
+	unsigned int guessed_heads;
+	unsigned int guessed_sectors;
+	unsigned int blocks;
+	unsigned int blocks_per_cyl;
+	int rc;
+
+	/* Guess geometry according to drive type */
+	if ( int13_is_fdd ( sandev ) ) {
+		if ( ( rc = int13_guess_geometry_fdd ( sandev, &guessed_heads,
+						       &guessed_sectors )) != 0)
+			return rc;
+	} else {
+		if ( ( rc = int13_guess_geometry_hdd ( sandev, scratch,
+						       &guessed_heads,
+						       &guessed_sectors )) != 0)
+			return rc;
+	}
+
+	/* Apply guesses if no geometry already specified */
+	if ( ! int13->heads )
+		int13->heads = guessed_heads;
+	if ( ! int13->sectors_per_track )
+		int13->sectors_per_track = guessed_sectors;
+	if ( ! int13->cylinders ) {
+		/* Avoid attempting a 64-bit divide on a 32-bit system */
+		blocks = int13_capacity32 ( sandev );
+		blocks_per_cyl = ( int13->heads * int13->sectors_per_track );
+		assert ( blocks_per_cyl != 0 );
+		int13->cylinders = ( blocks / blocks_per_cyl );
+		if ( int13->cylinders > 1024 )
+			int13->cylinders = 1024;
+	}
+
+	return 0;
+}
+#endif /* #if 0 */
+
+/**
+ * Update BIOS drive count
+ */
+void int13_sync_num_drives ( void ) {
+	struct san_device *sandev;
+	struct int13_data *int13;
+	uint8_t *counter;
+	uint8_t max_drive;
+	uint8_t required;
+
+	/* Get current drive counts */
+	get_real ( equipment_word, BDA_SEG, BDA_EQUIPMENT_WORD );
+	get_real ( num_drives, BDA_SEG, BDA_NUM_DRIVES );
+	num_fdds = ( ( equipment_word & 0x0001 ) ?
+		     ( ( ( equipment_word >> 6 ) & 0x3 ) + 1 ) : 0 );
+
+	/* Ensure count is large enough to cover all of our SAN devices */
+	for_each_sandev ( sandev ) {
+		int13 = sandev->priv;
+		counter = ( int13_is_fdd ( sandev ) ? &num_fdds : &num_drives );
+		max_drive = sandev->drive;
+		if ( max_drive < int13->natural_drive )
+			max_drive = int13->natural_drive;
+		required = ( ( max_drive & 0x7f ) + 1 );
+		if ( *counter < required ) {
+			*counter = required;
+			DBGC ( sandev, "INT13 drive %02x added to drive count: "
+			       "%d HDDs, %d FDDs\n",
+			       sandev->drive, num_drives, num_fdds );
+		}
+	}
+
+	/* Update current drive count */
+	equipment_word &= ~( ( 0x3 << 6 ) | 0x0001 );
+	if ( num_fdds ) {
+		equipment_word |= ( 0x0001 |
+				    ( ( ( num_fdds - 1 ) & 0x3 ) << 6 ) );
+	}
+	put_real ( equipment_word, BDA_SEG, BDA_EQUIPMENT_WORD );
+	put_real ( num_drives, BDA_SEG, BDA_NUM_DRIVES );
+}
+
+/**
+ * Check number of drives
+ */
+void int13_check_num_drives ( void ) {
+	uint16_t check_equipment_word;
+	uint8_t check_num_drives;
+
+	get_real ( check_equipment_word, BDA_SEG, BDA_EQUIPMENT_WORD );
+	get_real ( check_num_drives, BDA_SEG, BDA_NUM_DRIVES );
+	if ( ( check_equipment_word != equipment_word ) ||
+	     ( check_num_drives != num_drives ) ) {
+		int13_sync_num_drives();
+	}
+}
+
+/**
+ * INT 13, 00 - Reset disk system
+ *
+ * @v sandev		SAN device
+ * @ret status		Status code
+ */
+static int int13_reset ( struct san_device *sandev,
+			 struct i386_all_regs *ix86 __unused ) {
+	int rc;
+
+	DBGC2 ( sandev, "Reset drive\n" );
+
+	/* Reset SAN device */
+	if ( ( rc = sandev_reset ( sandev ) ) != 0 )
+		return -INT13_STATUS_RESET_FAILED;
+
+	return 0;
+}
+
+/**
+ * INT 13, 01 - Get status of last operation
+ *
+ * @v sandev		SAN device
+ * @ret status		Status code
+ */
+static int int13_get_last_status ( struct san_device *sandev,
+				   struct i386_all_regs *ix86 __unused ) {
+	struct int13_data *int13 = sandev->priv;
+
+	DBGC2 ( sandev, "Get status of last operation\n" );
+	return int13->last_status;
+}
+
+/**
+ * Read / write sectors
+ *
+ * @v sandev		SAN device
+ * @v al		Number of sectors to read or write (must be nonzero)
+ * @v ch		Low bits of cylinder number
+ * @v cl (bits 7:6)	High bits of cylinder number
+ * @v cl (bits 5:0)	Sector number
+ * @v dh		Head number
+ * @v es:bx		Data buffer
+ * @v sandev_rw		SAN device read/write method
+ * @ret status		Status code
+ * @ret al		Number of sectors read or written
+ */
+static int int13_rw_sectors ( struct san_device *sandev,
+			      struct i386_all_regs *ix86,
+			      int ( * sandev_rw ) ( struct san_device *sandev,
+						    uint64_t lba,
+						    unsigned int count,
+						    userptr_t buffer ) ) {
+	struct int13_data *int13 = sandev->priv;
+	unsigned int cylinder, head, sector;
+	unsigned long lba;
+	unsigned int count;
+	userptr_t buffer;
+	int rc;
+
+	/* Validate blocksize */
+	if ( sandev_blksize ( sandev ) != INT13_BLKSIZE ) {
+		DBGC ( sandev, "\nINT 13 drive %02x invalid blocksize (%zd) "
+		       "for non-extended read/write\n",
+		       sandev->drive, sandev_blksize ( sandev ) );
+		return -INT13_STATUS_INVALID;
+	}
+
+	/* Calculate parameters */
+	cylinder = ( ( ( ix86->regs.cl & 0xc0 ) << 2 ) | ix86->regs.ch );
+	head = ix86->regs.dh;
+	sector = ( ix86->regs.cl & 0x3f );
+	if ( ( cylinder >= int13->cylinders ) ||
+	     ( head >= int13->heads ) ||
+	     ( sector < 1 ) || ( sector > int13->sectors_per_track ) ) {
+		DBGC ( sandev, "C/H/S %d/%d/%d out of range for geometry "
+		       "%d/%d/%d\n", cylinder, head, sector, int13->cylinders,
+		       int13->heads, int13->sectors_per_track );
+		return -INT13_STATUS_INVALID;
+	}
+	lba = ( ( ( ( cylinder * int13->heads ) + head )
+		  * int13->sectors_per_track ) + sector - 1 );
+	count = ix86->regs.al;
+	buffer = real_to_user ( ix86->segs.es, ix86->regs.bx );
+
+	DBGC2 ( sandev, "C/H/S %d/%d/%d = LBA %08lx <-> %04x:%04x (count %d)\n",
+		cylinder, head, sector, lba, ix86->segs.es, ix86->regs.bx,
+		count );
+
+	/* Read from / write to block device */
+	if ( ( rc = sandev_rw ( sandev, lba, count, buffer ) ) != 0 ){
+		DBGC ( sandev, "INT13 drive %02x I/O failed: %s\n",
+		       sandev->drive, strerror ( rc ) );
+		return -INT13_STATUS_READ_ERROR;
+	}
+
+	return 0;
+}
+
+/**
+ * INT 13, 02 - Read sectors
+ *
+ * @v sandev		SAN device
+ * @v al		Number of sectors to read (must be nonzero)
+ * @v ch		Low bits of cylinder number
+ * @v cl (bits 7:6)	High bits of cylinder number
+ * @v cl (bits 5:0)	Sector number
+ * @v dh		Head number
+ * @v es:bx		Data buffer
+ * @ret status		Status code
+ * @ret al		Number of sectors read
+ */
+static int int13_read_sectors ( struct san_device *sandev,
+				struct i386_all_regs *ix86 ) {
+
+	DBGC2 ( sandev, "Read: " );
+	return int13_rw_sectors ( sandev, ix86, sandev_read );
+}
+
+/**
+ * INT 13, 03 - Write sectors
+ *
+ * @v sandev		SAN device
+ * @v al		Number of sectors to write (must be nonzero)
+ * @v ch		Low bits of cylinder number
+ * @v cl (bits 7:6)	High bits of cylinder number
+ * @v cl (bits 5:0)	Sector number
+ * @v dh		Head number
+ * @v es:bx		Data buffer
+ * @ret status		Status code
+ * @ret al		Number of sectors written
+ */
+static int int13_write_sectors ( struct san_device *sandev,
+				 struct i386_all_regs *ix86 ) {
+
+	DBGC2 ( sandev, "Write: " );
+	return int13_rw_sectors ( sandev, ix86, sandev_write );
+}
+
+/**
+ * INT 13, 08 - Get drive parameters
+ *
+ * @v sandev		SAN device
+ * @ret status		Status code
+ * @ret ch		Low bits of maximum cylinder number
+ * @ret cl (bits 7:6)	High bits of maximum cylinder number
+ * @ret cl (bits 5:0)	Maximum sector number
+ * @ret dh		Maximum head number
+ * @ret dl		Number of drives
+ */
+static int int13_get_parameters ( struct san_device *sandev,
+				  struct i386_all_regs *ix86 ) {
+	struct int13_data *int13 = sandev->priv;
+	unsigned int max_cylinder = int13->cylinders - 1;
+	unsigned int max_head = int13->heads - 1;
+	unsigned int max_sector = int13->sectors_per_track; /* sic */
+
+	DBGC2 ( sandev, "Get drive parameters\n" );
+
+	/* Validate blocksize */
+	if ( sandev_blksize ( sandev ) != INT13_BLKSIZE ) {
+		DBGC ( sandev, "\nINT 13 drive %02x invalid blocksize (%zd) "
+		       "for non-extended parameters\n",
+		       sandev->drive, sandev_blksize ( sandev ) );
+		return -INT13_STATUS_INVALID;
+	}
+
+	/* Common parameters */
+	ix86->regs.ch = ( max_cylinder & 0xff );
+	ix86->regs.cl = ( ( ( max_cylinder >> 8 ) << 6 ) | max_sector );
+	ix86->regs.dh = max_head;
+	ix86->regs.dl = ( int13_is_fdd ( sandev ) ? num_fdds : num_drives );
+
+	/* Floppy-specific parameters */
+	if ( int13_is_fdd ( sandev ) ) {
+		ix86->regs.bl = INT13_FDD_TYPE_1M44;
+		ix86->segs.es = rm_ds;
+		ix86->regs.di = __from_data16 ( &int13_fdd_params );
+	}
+
+	return 0;
+}
+
+/**
+ * INT 13, 15 - Get disk type
+ *
+ * @v sandev		SAN device
+ * @ret ah		Type code
+ * @ret cx:dx		Sector count
+ * @ret status		Status code / disk type
+ */
+static int int13_get_disk_type ( struct san_device *sandev,
+				 struct i386_all_regs *ix86 ) {
+	uint32_t blocks;
+
+	DBGC2 ( sandev, "Get disk type\n" );
+
+	if ( int13_is_fdd ( sandev ) ) {
+		return INT13_DISK_TYPE_FDD;
+	} else {
+		blocks = int13_capacity32 ( sandev );
+		ix86->regs.cx = ( blocks >> 16 );
+		ix86->regs.dx = ( blocks & 0xffff );
+		return INT13_DISK_TYPE_HDD;
+	}
+}
+
+/**
+ * INT 13, 41 - Extensions installation check
+ *
+ * @v sandev		SAN device
+ * @v bx		0x55aa
+ * @ret bx		0xaa55
+ * @ret cx		Extensions API support bitmap
+ * @ret status		Status code / API version
+ */
+static int int13_extension_check ( struct san_device *sandev __unused,
+				   struct i386_all_regs *ix86 ) {
+
+	if ( ix86->regs.bx == 0x55aa ) {
+		DBGC2 ( sandev, "INT13 extensions installation check\n" );
+		ix86->regs.bx = 0xaa55;
+		ix86->regs.cx = ( INT13_EXTENSION_LINEAR |
+				  INT13_EXTENSION_EDD |
+				  INT13_EXTENSION_64BIT );
+		return INT13_EXTENSION_VER_3_0;
+	} else {
+		return -INT13_STATUS_INVALID;
+	}
+}
+
+/**
+ * Extended read / write
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Disk address packet
+ * @v sandev_rw		SAN device read/write method
+ * @ret status		Status code
+ */
+static int int13_extended_rw ( struct san_device *sandev,
+			       struct i386_all_regs *ix86,
+			       int ( * sandev_rw ) ( struct san_device *sandev,
+						     uint64_t lba,
+						     unsigned int count,
+						     userptr_t buffer ) ) {
+	struct int13_disk_address addr;
+	uint8_t bufsize;
+	uint64_t lba;
+	unsigned long count;
+	userptr_t buffer;
+	int rc;
+
+	/* Extended reads are not allowed on floppy drives.
+	 * ELTORITO.SYS seems to assume that we are really a CD-ROM if
+	 * we support extended reads for a floppy drive.
+	 */
+	if ( int13_is_fdd ( sandev ) )
+		return -INT13_STATUS_INVALID;
+
+	/* Get buffer size */
+	get_real ( bufsize, ix86->segs.ds,
+		   ( ix86->regs.si + offsetof ( typeof ( addr ), bufsize ) ) );
+	if ( bufsize < offsetof ( typeof ( addr ), buffer_phys ) ) {
+		DBGC2 ( sandev, "<invalid buffer size %#02x\n>\n", bufsize );
+		return -INT13_STATUS_INVALID;
+	}
+
+	/* Read parameters from disk address structure */
+	memset ( &addr, 0, sizeof ( addr ) );
+	copy_from_real ( &addr, ix86->segs.ds, ix86->regs.si, bufsize );
+	lba = addr.lba;
+	DBGC2 ( sandev, "LBA %08llx <-> ", ( ( unsigned long long ) lba ) );
+	if ( ( addr.count == 0xff ) ||
+	     ( ( addr.buffer.segment == 0xffff ) &&
+	       ( addr.buffer.offset == 0xffff ) ) ) {
+		buffer = phys_to_user ( addr.buffer_phys );
+		DBGC2 ( sandev, "%08llx",
+			( ( unsigned long long ) addr.buffer_phys ) );
+	} else {
+		buffer = real_to_user ( addr.buffer.segment,
+					addr.buffer.offset );
+		DBGC2 ( sandev, "%04x:%04x", addr.buffer.segment,
+			addr.buffer.offset );
+	}
+	if ( addr.count <= 0x7f ) {
+		count = addr.count;
+	} else if ( addr.count == 0xff ) {
+		count = addr.long_count;
+	} else {
+		DBGC2 ( sandev, " <invalid count %#02x>\n", addr.count );
+		return -INT13_STATUS_INVALID;
+	}
+	DBGC2 ( sandev, " (count %ld)\n", count );
+
+	/* Read from / write to block device */
+	if ( ( rc = sandev_rw ( sandev, lba, count, buffer ) ) != 0 ) {
+		DBGC ( sandev, "INT13 drive %02x extended I/O failed: %s\n",
+		       sandev->drive, strerror ( rc ) );
+		/* Record that no blocks were transferred successfully */
+		addr.count = 0;
+		put_real ( addr.count, ix86->segs.ds,
+			   ( ix86->regs.si +
+			     offsetof ( typeof ( addr ), count ) ) );
+		return -INT13_STATUS_READ_ERROR;
+	}
+
+    copy_to_real (ix86->segs.ds, ix86->regs.si, &addr, bufsize );
+
+	return 0;
+}
+
+/**
+ * INT 13, 42 - Extended read
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Disk address packet
+ * @ret status		Status code
+ */
+static int int13_extended_read ( struct san_device *sandev,
+				 struct i386_all_regs *ix86 ) {
+
+	DBGC2 ( sandev, "Extended read: " );
+	return int13_extended_rw ( sandev, ix86, sandev_read );
+}
+
+/**
+ * INT 13, 43 - Extended write
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Disk address packet
+ * @ret status		Status code
+ */
+static int int13_extended_write ( struct san_device *sandev,
+				  struct i386_all_regs *ix86 ) {
+
+	DBGC2 ( sandev, "Extended write: " );
+	return int13_extended_rw ( sandev, ix86, sandev_write );
+}
+
+/**
+ * INT 13, 44 - Verify sectors
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Disk address packet
+ * @ret status		Status code
+ */
+static int int13_extended_verify ( struct san_device *sandev,
+				   struct i386_all_regs *ix86 ) {
+	struct int13_disk_address addr;
+	uint64_t lba;
+	unsigned long count;
+
+	/* Read parameters from disk address structure */
+	if ( DBG_EXTRA ) {
+		copy_from_real ( &addr, ix86->segs.ds, ix86->regs.si,
+				 sizeof ( addr ));
+		lba = addr.lba;
+		count = addr.count;
+		DBGC2 ( sandev, "Verify: LBA %08llx (count %ld)\n",
+			( ( unsigned long long ) lba ), count );
+	}
+
+	/* We have no mechanism for verifying sectors */
+	return -INT13_STATUS_INVALID;
+}
+
+/**
+ * INT 13, 44 - Extended seek
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Disk address packet
+ * @ret status		Status code
+ */
+static int int13_extended_seek ( struct san_device *sandev,
+				 struct i386_all_regs *ix86 ) {
+	struct int13_disk_address addr;
+	uint64_t lba;
+	unsigned long count;
+
+	/* Read parameters from disk address structure */
+	if ( DBG_EXTRA ) {
+		copy_from_real ( &addr, ix86->segs.ds, ix86->regs.si,
+				 sizeof ( addr ));
+		lba = addr.lba;
+		count = addr.count;
+		DBGC2 ( sandev, "Seek: LBA %08llx (count %ld)\n",
+			( ( unsigned long long ) lba ), count );
+	}
+
+	/* Ignore and return success */
+	return 0;
+}
+
+/**
+ * Build device path information
+ *
+ * @v sandev		SAN device
+ * @v dpi		Device path information
+ * @ret rc		Return status code
+ */
+static int int13_device_path_info ( struct san_device *sandev,
+				    struct edd_device_path_information *dpi ) {
+	struct san_path *sanpath;
+	struct device *device;
+	struct device_description *desc;
+	unsigned int i;
+	uint8_t sum = 0;
+	int rc;
+    return -ECANCELED;
+	/* Reopen block device if necessary */
+	if ( sandev_needs_reopen ( sandev ) &&
+	     ( ( rc = sandev_reopen ( sandev ) ) != 0 ) )
+		return rc;
+	sanpath = sandev->active;
+	assert ( sanpath != NULL );
+
+	/* Get underlying hardware device */
+	device = identify_device ( &sanpath->block );
+	if ( ! device ) {
+		DBGC ( sandev, "INT13 drive %02x cannot identify hardware "
+		       "device\n", sandev->drive );
+		return -ENODEV;
+	}
+
+	/* Fill in bus type and interface path */
+	desc = &device->desc;
+	switch ( desc->bus_type ) {
+	case BUS_TYPE_PCI:
+		dpi->host_bus_type.type = EDD_BUS_TYPE_PCI;
+		dpi->interface_path.pci.bus = PCI_BUS ( desc->location );
+		dpi->interface_path.pci.slot = PCI_SLOT ( desc->location );
+		dpi->interface_path.pci.function = PCI_FUNC ( desc->location );
+		dpi->interface_path.pci.channel = 0xff; /* unused */
+		break;
+	default:
+		DBGC ( sandev, "INT13 drive %02x unrecognised bus type %d\n",
+		       sandev->drive, desc->bus_type );
+		return -ENOTSUP;
+	}
+
+	/* Get EDD block device description */
+	if ( ( rc = edd_describe ( &sanpath->block, &dpi->interface_type,
+				   &dpi->device_path ) ) != 0 ) {
+		DBGC ( sandev, "INT13 drive %02x cannot identify block device: "
+		       "%s\n", sandev->drive, strerror ( rc ) );
+		return rc;
+	}
+
+	/* Fill in common fields and fix checksum */
+	dpi->key = EDD_DEVICE_PATH_INFO_KEY;
+	dpi->len = sizeof ( *dpi );
+	for ( i = 0 ; i < sizeof ( *dpi ) ; i++ )
+		sum += *( ( ( uint8_t * ) dpi ) + i );
+	dpi->checksum -= sum;
+
+	return 0;
+}
+
+/**
+ * INT 13, 48 - Get extended parameters
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Drive parameter table
+ * @ret status		Status code
+ */
+static int int13_get_extended_parameters ( struct san_device *sandev,
+					   struct i386_all_regs *ix86 ) {
+	struct int13_data *int13 = sandev->priv;
+	struct int13_disk_parameters params;
+	struct segoff address;
+	size_t len = sizeof ( params );
+	uint16_t bufsize;
+	int rc;
+
+	/* Get buffer size */
+	get_real ( bufsize, ix86->segs.ds,
+		   ( ix86->regs.si + offsetof ( typeof ( params ), bufsize )));
+
+	DBGC2 ( sandev, "Get extended drive parameters to %04x:%04x+%02x\n",
+		ix86->segs.ds, ix86->regs.si, bufsize );
+
+	/* Build drive parameters */
+	memset ( &params, 0, sizeof ( params ) );
+	params.flags = INT13_FL_DMA_TRANSPARENT;
+	if ( ( int13->cylinders < 1024 ) &&
+	     ( sandev_capacity ( sandev ) <= INT13_MAX_CHS_SECTORS ) ) {
+		params.flags |= INT13_FL_CHS_VALID;
+	}
+	params.cylinders = int13->cylinders;
+	params.heads = int13->heads;
+	params.sectors_per_track = int13->sectors_per_track;
+	params.sectors = sandev_capacity ( sandev );
+	params.sector_size = sandev_blksize ( sandev );
+	memset ( &params.dpte, 0xff, sizeof ( params.dpte ) );
+	if ( ( rc = int13_device_path_info ( sandev, &params.dpi ) ) != 0 ) {
+		DBGC ( sandev, "INT13 drive %02x could not provide device "
+		       "path information: %s\n",
+		       sandev->drive, strerror ( rc ) );
+		len = offsetof ( typeof ( params ), dpi );
+	}
+
+	/* Calculate returned "buffer size" (which will be less than
+	 * the length actually copied if device path information is
+	 * present).
+	 */
+	if ( bufsize < offsetof ( typeof ( params ), dpte ) )
+		return -INT13_STATUS_INVALID;
+	if ( bufsize < offsetof ( typeof ( params ), dpi ) ) {
+		params.bufsize = offsetof ( typeof ( params ), dpte );
+	} else {
+		params.bufsize = offsetof ( typeof ( params ), dpi );
+	}
+
+	DBGC ( sandev, "INT 13 drive %02x described using extended "
+	       "parameters:\n", sandev->drive );
+	address.segment = ix86->segs.ds;
+	address.offset = ix86->regs.si;
+	DBGC_HDA ( sandev, address, &params, len );
+
+	/* Return drive parameters */
+	if ( len > bufsize )
+		len = bufsize;
+	copy_to_real ( ix86->segs.ds, ix86->regs.si, &params, len );
+
+	return 0;
+}
+
+/**
+ * INT 13, 4b - Get status or terminate CD-ROM emulation
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Specification packet
+ * @ret status		Status code
+ */
+static int int13_cdrom_status_terminate ( struct san_device *sandev,
+					  struct i386_all_regs *ix86 ) {
+	struct int13_cdrom_specification specification;
+
+	DBGC2 ( sandev, "Get CD-ROM emulation status to %04x:%04x%s\n",
+		ix86->segs.ds, ix86->regs.si,
+		( ix86->regs.al ? "" : " and terminate" ) );
+
+	/* Fail if we are not a CD-ROM */
+	if ( ! sandev->is_cdrom ) {
+		DBGC ( sandev, "INT13 drive %02x is not a CD-ROM\n",
+		       sandev->drive );
+		return -INT13_STATUS_INVALID;
+	}
+
+	/* Build specification packet */
+	memset ( &specification, 0, sizeof ( specification ) );
+	specification.size = sizeof ( specification );
+	specification.drive = sandev->drive;
+
+	/* Return specification packet */
+	copy_to_real ( ix86->segs.ds, ix86->regs.si, &specification,
+		       sizeof ( specification ) );
+
+	return 0;
+}
+
+
+/**
+ * INT 13, 4d - Read CD-ROM boot catalog
+ *
+ * @v sandev		SAN device
+ * @v ds:si		Command packet
+ * @ret status		Status code
+ */
+static int int13_cdrom_read_boot_catalog ( struct san_device *sandev,
+					   struct i386_all_regs *ix86 ) {
+	struct int13_data *int13 = sandev->priv;
+	struct int13_cdrom_boot_catalog_command command;
+	unsigned int start;
+	int rc;
+
+	/* Read parameters from command packet */
+	copy_from_real ( &command, ix86->segs.ds, ix86->regs.si,
+			 sizeof ( command ) );
+	DBGC2 ( sandev, "Read CD-ROM boot catalog to %08x\n", command.buffer );
+
+	/* Fail if we have no boot catalog */
+	if ( ! int13->boot_catalog ) {
+		DBGC ( sandev, "INT13 drive %02x has no boot catalog\n",
+		       sandev->drive );
+		return -INT13_STATUS_INVALID;
+	}
+	start = ( int13->boot_catalog + command.start );
+
+	/* Read from boot catalog */
+	if ( ( rc = sandev_read ( sandev, start, command.count,
+				  phys_to_user ( command.buffer ) ) ) != 0 ) {
+		DBGC ( sandev, "INT13 drive %02x could not read boot catalog: "
+		       "%s\n", sandev->drive, strerror ( rc ) );
+		return -INT13_STATUS_READ_ERROR;
+	}
+
+	return 0;
+}
+
+
+
+/**
+ * INT 13 handler
+ *
+ */
+static __asmcall void int13 ( struct i386_all_regs *ix86 ) {
+	int command = ix86->regs.ah;
+	unsigned int bios_drive = ix86->regs.dl;
+	struct san_device *sandev;
+	struct int13_data *int13;
+	int status;
+
+	/* We simulate a cdrom, so no need to sync hd drive number */
+	//int13_check_num_drives();
+
+    if (bios_drive == VENTOY_BIOS_FAKE_DRIVE)
+    {
+        ix86->regs.dl = g_sandev->exdrive;
+        return;
+    }
+
+    // drive swap
+    if (g_drive_map1 >= 0x80 && g_drive_map2 >= 0x80)
+    {
+        if (bios_drive == g_drive_map1)
+        {
+            ix86->regs.dl = g_drive_map2;
+            return;
+        }
+        else if (bios_drive == g_drive_map2)
+        {
+            ix86->regs.dl = g_drive_map1;
+            return;
+        }
+    }
+
+	for_each_sandev ( sandev ) {
+
+		int13 = sandev->priv;
+		if ( bios_drive != sandev->drive ) {
+			/* Remap any accesses to this drive's natural number */
+			if ( bios_drive == int13->natural_drive ) {
+				DBGC2 ( sandev, "INT13,%02x (%02x) remapped to "
+					"(%02x)\n", ix86->regs.ah,
+					bios_drive, sandev->drive );
+				ix86->regs.dl = sandev->drive;
+				return;
+			} else if ( ( ( bios_drive & 0x7f ) == 0x7f ) &&
+				    ( command == INT13_CDROM_STATUS_TERMINATE )
+				    && sandev->is_cdrom ) {
+                    
+				/* Catch non-drive-specific CD-ROM calls */
+			} else {
+				return;
+			}
+		}
+
+        sandev->int13_command = command;
+        sandev->x86_regptr = ix86;        
+
+		DBGC2 ( sandev, "INT13,%02x (%02x): ",
+		  	ix86->regs.ah, bios_drive );
+
+		switch ( command ) {
+		case INT13_RESET:
+			status = int13_reset ( sandev, ix86 );
+			break;
+		case INT13_GET_LAST_STATUS:
+			status = int13_get_last_status ( sandev, ix86 );
+			break;
+		case INT13_READ_SECTORS:
+			status = int13_read_sectors ( sandev, ix86 );
+			break;
+		case INT13_WRITE_SECTORS:
+			status = int13_write_sectors ( sandev, ix86 );
+			break;
+		case INT13_GET_PARAMETERS:
+			status = int13_get_parameters ( sandev, ix86 );
+			break;
+		case INT13_GET_DISK_TYPE:
+			status = int13_get_disk_type ( sandev, ix86 );
+			break;
+		case INT13_EXTENSION_CHECK:
+			status = int13_extension_check ( sandev, ix86 );
+			break;
+		case INT13_EXTENDED_READ:
+			status = int13_extended_read ( sandev, ix86 );
+			break;
+		case INT13_EXTENDED_WRITE:
+			status = int13_extended_write ( sandev, ix86 );
+			break;
+		case INT13_EXTENDED_VERIFY:
+			status = int13_extended_verify ( sandev, ix86 );
+			break;
+		case INT13_EXTENDED_SEEK:
+			status = int13_extended_seek ( sandev, ix86 );
+			break;
+		case INT13_GET_EXTENDED_PARAMETERS:
+			status = int13_get_extended_parameters ( sandev, ix86 );
+			break;
+		case INT13_CDROM_STATUS_TERMINATE:
+			status = int13_cdrom_status_terminate ( sandev, ix86 );
+			break;
+		case INT13_CDROM_READ_BOOT_CATALOG:
+			status = int13_cdrom_read_boot_catalog ( sandev, ix86 );
+			break;
+		default:
+			DBGC2 ( sandev, "*** Unrecognised INT13 ***\n" );
+			status = -INT13_STATUS_INVALID;
+			break;
+		}
+
+		/* Store status for INT 13,01 */
+		int13->last_status = status;
+
+		/* Negative status indicates an error */
+		if ( status < 0 ) {
+			status = -status;
+			DBGC ( sandev, "INT13,%02x (%02x) failed with status "
+			       "%02x\n", ix86->regs.ah, sandev->drive, status );
+		} else {
+			ix86->flags &= ~CF;
+		}
+		ix86->regs.ah = status;
+
+		/* Set OF to indicate to wrapper not to chain this call */
+		ix86->flags |= OF;
+
+		return;
+	}
+}
+
+/**
+ * Hook INT 13 handler
+ *
+ */
+static void int13_hook_vector ( void ) {
+	/* Assembly wrapper to call int13().  int13() sets OF if we
+	 * should not chain to the previous handler.  (The wrapper
+	 * clears CF and OF before calling int13()).
+	 */
+	__asm__  __volatile__ (
+	       TEXT16_CODE ( "\nint13_wrapper:\n\t"
+			     /* Preserve %ax and %dx for future reference */
+			     "pushw %%bp\n\t"
+			     "movw %%sp, %%bp\n\t"			     
+			     "pushw %%ax\n\t"
+			     "pushw %%dx\n\t"
+			     /* Clear OF, set CF, call int13() */
+			     "orb $0, %%al\n\t" 
+			     "stc\n\t"
+			     VIRT_CALL ( int13 )
+			     /* Chain if OF not set */
+			     "jo 1f\n\t"
+			     "pushfw\n\t"
+			     "lcall *%%cs:int13_vector\n\t"
+			     "\n1:\n\t"
+			     /* Overwrite flags for iret */
+			     "pushfw\n\t"
+			     "popw 6(%%bp)\n\t"
+			     /* Fix up %dl:
+			      *
+			      * INT 13,15 : do nothing if hard disk
+			      * INT 13,08 : load with number of drives
+			      * all others: restore original value
+			      */
+			     "cmpb $0x15, -1(%%bp)\n\t"
+			     "jne 2f\n\t"
+			     "testb $0x80, -4(%%bp)\n\t"
+			     "jnz 3f\n\t"
+			     "\n2:\n\t"
+			     "movb -4(%%bp), %%dl\n\t"
+			     "cmpb $0x08, -1(%%bp)\n\t"
+			     "jne 3f\n\t"
+			     "testb $0x80, %%dl\n\t"
+			     "movb %%cs:num_drives, %%dl\n\t"
+			     "jnz 3f\n\t"
+			     "movb %%cs:num_fdds, %%dl\n\t"
+			     /* Return */
+			     "\n3:\n\t"
+			     "movw %%bp, %%sp\n\t"
+			     "popw %%bp\n\t"
+			     "iret\n\t" ) : : );
+
+	hook_bios_interrupt ( 0x13, ( intptr_t ) int13_wrapper, &int13_vector );
+}
+
+
+
+/**
+ * Load and verify master boot record from INT 13 drive
+ *
+ * @v drive		Drive number
+ * @v address		Boot code address to fill in
+ * @ret rc		Return status code
+ */
+static int int13_load_mbr ( unsigned int drive, struct segoff *address ) {
+	uint16_t status;
+	int discard_b, discard_c, discard_d;
+	uint16_t magic;
+
+	/* Use INT 13, 02 to read the MBR */
+	address->segment = 0;
+	address->offset = 0x7c00;
+	__asm__ __volatile__ ( REAL_CODE ( "pushw %%es\n\t"
+					   "pushl %%ebx\n\t"
+					   "popw %%bx\n\t"
+					   "popw %%es\n\t"
+					   "stc\n\t"
+					   "sti\n\t"
+					   "int $0x13\n\t"
+					   "sti\n\t" /* BIOS bugs */
+					   "jc 1f\n\t"
+					   "xorw %%ax, %%ax\n\t"
+					   "\n1:\n\t"
+					   "popw %%es\n\t" )
+			       : "=a" ( status ), "=b" ( discard_b ),
+				 "=c" ( discard_c ), "=d" ( discard_d )
+			       : "a" ( 0x0201 ), "b" ( *address ),
+				 "c" ( 1 ), "d" ( drive ) );
+	if ( status ) {
+		DBG ( "INT13 drive %02x could not read MBR (status %04x)\n",
+		      drive, status );
+		return -EIO;
+	}
+
+	/* Check magic signature */
+	get_real ( magic, address->segment,
+		   ( address->offset +
+		     offsetof ( struct master_boot_record, magic ) ) );
+	if ( magic != INT13_MBR_MAGIC ) {
+		DBG ( "INT13 drive %02x does not contain a valid MBR\n",
+		      drive );
+		return -ENOEXEC;
+	}
+
+	return 0;
+}
+
+/** El Torito boot catalog command packet */
+static struct int13_cdrom_boot_catalog_command __data16 ( eltorito_cmd ) = {
+	.size = sizeof ( struct int13_cdrom_boot_catalog_command ),
+	.count = 1,
+	.buffer = 0x7c00,
+	.start = 0,
+};
+#define eltorito_cmd __use_data16 ( eltorito_cmd )
+
+/** El Torito disk address packet */
+static struct int13_disk_address __bss16 ( eltorito_address );
+#define eltorito_address __use_data16 ( eltorito_address )
+
+/**
+ * Load and verify El Torito boot record from INT 13 drive
+ *
+ * @v drive		Drive number
+ * @v address		Boot code address to fill in
+ * @ret rc		Return status code
+ */
+static int int13_load_eltorito ( unsigned int drive, struct segoff *address ) {
+	struct {
+		struct eltorito_validation_entry valid;
+		struct eltorito_boot_entry boot;
+	} __attribute__ (( packed )) catalog;
+	uint16_t status;
+    
+    if (g_sandev && g_sandev->drive == drive)
+    {
+        copy_to_user(phys_to_user ( eltorito_cmd.buffer ), 0, g_sandev->boot_catalog_sector, sizeof(g_sandev->boot_catalog_sector));
+    }
+    else
+    {
+    	/* Use INT 13, 4d to read the boot catalog */
+    	__asm__ __volatile__ ( REAL_CODE ( "stc\n\t"
+    					   "sti\n\t"
+    					   "int $0x13\n\t"
+    					   "sti\n\t" /* BIOS bugs */
+    					   "jc 1f\n\t"
+    					   "xorw %%ax, %%ax\n\t"
+    					   "\n1:\n\t" )
+    			       : "=a" ( status )
+    			       : "a" ( 0x4d00 ), "d" ( drive ),
+    				 "S" ( __from_data16 ( &eltorito_cmd ) ) );
+    	if ( status ) {
+    		DBG ( "INT13 drive %02x could not read El Torito boot catalog "
+    		      "(status %04x)\n", drive, status );
+    		return -EIO;
+    	}
+    }
+    
+	copy_from_user ( &catalog, phys_to_user ( eltorito_cmd.buffer ), 0,
+			 sizeof ( catalog ) );
+
+	/* Sanity checks */
+	if ( catalog.valid.platform_id != ELTORITO_PLATFORM_X86 ) {
+		DBG ( "INT13 drive %02x El Torito specifies unknown platform "
+		      "%02x\n", drive, catalog.valid.platform_id );
+		return -ENOEXEC;
+	}
+	if ( catalog.boot.indicator != ELTORITO_BOOTABLE ) {
+		DBG ( "INT13 drive %02x El Torito is not bootable\n", drive );
+		return -ENOEXEC;
+	}
+	if ( catalog.boot.media_type != ELTORITO_NO_EMULATION ) {
+		DBG ( "INT13 drive %02x El Torito requires emulation "
+		       "type %02x\n", drive, catalog.boot.media_type );
+		return -ENOTSUP;
+	}
+	DBG ( "INT13 drive %02x El Torito boot image at LBA %08x (count %d)\n",
+	      drive, catalog.boot.start, catalog.boot.length );
+	address->segment = ( catalog.boot.load_segment ?
+			     catalog.boot.load_segment : 0x7c0 );
+	address->offset = 0;
+	DBG ( "INT13 drive %02x El Torito boot image loads at %04x:%04x\n",
+	      drive, address->segment, address->offset );
+
+	/* Use INT 13, 42 to read the boot image */
+	eltorito_address.bufsize =
+		offsetof ( typeof ( eltorito_address ), buffer_phys );
+	eltorito_address.count = catalog.boot.length;
+	eltorito_address.buffer = *address;
+	eltorito_address.lba = catalog.boot.start;
+	__asm__ __volatile__ ( REAL_CODE ( "stc\n\t"
+					   "sti\n\t"
+					   "int $0x13\n\t"
+					   "sti\n\t" /* BIOS bugs */
+					   "jc 1f\n\t"
+					   "xorw %%ax, %%ax\n\t"
+					   "\n1:\n\t" )
+			       : "=a" ( status )
+			       : "a" ( 0x4200 ), "d" ( drive ),
+				 "S" ( __from_data16 ( &eltorito_address ) ) );
+	if ( status ) {
+		DBG ( "INT13 drive %02x could not read El Torito boot image "
+		      "(status %04x)\n", drive, status );
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/**
+ * Hook INT 13 SAN device
+ *
+ * @v drive		Drive number
+ * @v uris		List of URIs
+ * @v count		Number of URIs
+ * @v flags		Flags
+ * @ret drive		Drive number, or negative error
+ *
+ * Registers the drive with the INT 13 emulation subsystem, and hooks
+ * the INT 13 interrupt vector (if not already hooked).
+ */
+unsigned int ventoy_int13_hook (ventoy_chain_head *chain) 
+{
+	unsigned int blocks;
+	unsigned int blocks_per_cyl;
+	unsigned int natural_drive;
+    struct int13_data *int13;
+
+	/* We simulate a cdrom, so no need to sync hd drive number */
+	//int13_sync_num_drives();
+
+    /* hook will copy num_drives to dl when int13 08 was called, so must initialize it's value */
+	get_real(num_drives, BDA_SEG, BDA_NUM_DRIVES);
+    
+	//natural_drive = num_drives | 0x80;
+	natural_drive = 0xE0; /* just set a cdrom drive number 224 */
+
+    if (chain->disk_drive >= 0x80 && chain->drive_map >= 0x80)
+    {
+        g_drive_map1 = chain->disk_drive;
+        g_drive_map2 = chain->drive_map;
+    }
+
+    /* a fake sandev */
+    g_sandev = zalloc(sizeof(struct san_device) + sizeof(struct int13_data));
+    g_sandev->priv = int13 = (struct int13_data *)(g_sandev + 1);
+    g_sandev->drive = int13->natural_drive = natural_drive;
+    g_sandev->is_cdrom = 1;
+    g_sandev->blksize_shift = 2;
+    g_sandev->capacity.blksize = 512;
+    g_sandev->capacity.blocks = chain->virt_img_size_in_bytes / 512;
+    g_sandev->exdrive = chain->disk_drive;
+    int13->boot_catalog = chain->boot_catalog;
+    memcpy(g_sandev->boot_catalog_sector, chain->boot_catalog_sector, sizeof(chain->boot_catalog_sector));
+
+
+    /* Apply guesses if no geometry already specified */
+	if ( ! int13->heads )
+		int13->heads = 255;
+	if ( ! int13->sectors_per_track )
+		int13->sectors_per_track = 63;
+	if ( ! int13->cylinders ) {
+		/* Avoid attempting a 64-bit divide on a 32-bit system */
+		blocks = int13_capacity32 ( g_sandev );
+		blocks_per_cyl = ( int13->heads * int13->sectors_per_track );
+
+		int13->cylinders = ( blocks / blocks_per_cyl );
+		if ( int13->cylinders > 1024 )
+			int13->cylinders = 1024;
+	}
+
+	/* Hook INT 13 vector if not already hooked */
+	int13_hook_vector();
+
+	/* Update BIOS drive count */
+	//int13_sync_num_drives();
+
+	return natural_drive;
+}
+
+static uint8_t __bss16_array ( xbftab, [512 + 128])
+        __attribute__ (( aligned ( 16 ) ));
+#define xbftab __use_data16 ( xbftab )
+
+void * ventoy_get_runtime_addr(void)
+{
+    return (void *)user_to_phys((userptr_t)(&xbftab), 0);
+}
+
+/**
+ * Attempt to boot from an INT 13 drive
+ *
+ * @v drive		Drive number
+ * @v filename		Filename (or NULL to use default)
+ * @ret rc		Return status code
+ *
+ * This boots from the specified INT 13 drive by loading the Master
+ * Boot Record to 0000:7c00 and jumping to it.  INT 18 is hooked to
+ * capture an attempt by the MBR to boot the next device.  (This is
+ * the closest thing to a return path from an MBR).
+ *
+ * Note that this function can never return success, by definition.
+ */
+int ventoy_int13_boot ( unsigned int drive, void *imginfo, const char *cmdline) {
+	//struct memory_map memmap;
+	int rc;
+    int headlen;
+	struct segoff address;
+    struct acpi_header *acpi = NULL; 
+    struct ibft_table *ibft = NULL;
+        
+	/* Look for a usable boot sector */
+	if ( ( ( rc = int13_load_eltorito ( drive, &address ) ) != 0 ) &&
+        ( ( rc = int13_load_mbr ( drive, &address ) ) != 0 ))
+		return rc;
+
+    if (imginfo)
+    {
+        if (strstr(cmdline, "ibft"))
+        {
+            headlen = 80;
+            ibft = (struct ibft_table *)(&xbftab);
+            acpi = &(ibft->acpi);
+            memset(ibft, 0, headlen);
+
+            acpi->signature = IBFT_SIG;
+            acpi->length = headlen + sizeof(ventoy_os_param);
+            acpi->revision = 1;
+            strncpy(acpi->oem_id, "ventoy", sizeof(acpi->oem_id));
+    	    strncpy(acpi->oem_table_id, "runtime", sizeof(acpi->oem_table_id));
+            memcpy((uint8_t *)ibft + headlen, imginfo, sizeof(ventoy_os_param));
+            acpi_fix_checksum(acpi);
+        }
+        else
+        {
+            memcpy((&xbftab), imginfo, sizeof(ventoy_os_param));
+        }
+    }
+
+	/* Dump out memory map prior to boot, if memmap debugging is
+	 * enabled.  Not required for program flow, but we have so
+	 * many problems that turn out to be memory-map related that
+	 * it's worth doing.
+	 */
+	//get_memmap ( &memmap );
+
+    DBGC(g_sandev, "start to boot ...\n");
+
+	/* Jump to boot sector */
+	if ( ( rc = call_bootsector ( address.segment, address.offset,
+				      drive ) ) != 0 ) {
+		return rc;
+	}
+
+	return -ECANCELED; /* -EIMPOSSIBLE */
+}
+

IPXE/ipxe-3fe683e/src/arch/x86/interface/pcbios/ventoy_int13.h

+
+#ifndef __VENTOY_INT13_H__
+#define __VENTOY_INT13_H__
+
+#undef for_each_sandev
+#define for_each_sandev( sandev ) sandev = g_sandev; if (sandev)
+
+int ventoy_vdisk_read(struct san_device*sandev, uint64_t lba, unsigned int count, unsigned long buffer);
+
+static inline int ventoy_sandev_write ( struct san_device *sandev, uint64_t lba, unsigned int count, unsigned long buffer )
+{
+    (void)sandev;
+    (void)lba;
+    (void)count;
+    (void)buffer;
+    DBGC(sandev, "ventoy_sandev_write\n");
+    return 0;
+}
+
+static inline int ventoy_sandev_reset (void *sandev)
+{
+    (void)sandev;
+    DBGC(sandev, "ventoy_sandev_reset\n");
+    return 0;
+}
+
+#define sandev_reset  ventoy_sandev_reset
+#define sandev_read   ventoy_vdisk_read
+#define sandev_write  ventoy_sandev_write
+
+#undef  ECANCELED
+#define ECANCELED 0x0b
+#undef  ENODEV
+#define ENODEV 0x2c
+#undef  ENOTSUP
+#define ENOTSUP 0x3c
+#undef  ENOMEM
+#define ENOMEM 0x31
+#undef  EIO
+#define EIO 0x1d
+#undef  ENOEXEC
+#define ENOEXEC 0x2e
+#undef  ENOSPC
+#define ENOSPC 0x34
+
+
+#endif /* __VENTOY_INT13_H__ */
+

IPXE/ipxe-3fe683e/src/config/settings.h

+#ifndef CONFIG_SETTINGS_H
+#define CONFIG_SETTINGS_H
+
+/** @file
+ *
+ * Configuration settings sources
+ *
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+//#define	PCI_SETTINGS	/* PCI device settings */
+//#define	CPUID_SETTINGS	/* CPUID settings */
+//#define	MEMMAP_SETTINGS	/* Memory map settings */
+//#define	VMWARE_SETTINGS	/* VMware GuestInfo settings */
+//#define	VRAM_SETTINGS	/* Video RAM dump settings */
+//#define	ACPI_SETTINGS	/* ACPI settings */
+
+#include <config/named.h>
+#include NAMED_CONFIG(settings.h)
+#include <config/local/settings.h>
+#include LOCAL_NAMED_CONFIG(settings.h)
+
+#endif /* CONFIG_SETTINGS_H */

IPXE/ipxe-3fe683e/src/core/device.c

+/*
+ * Copyright (C) 2006 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <string.h>
+#include <ipxe/list.h>
+#include <ipxe/tables.h>
+#include <ipxe/init.h>
+#include <ipxe/interface.h>
+#include <ipxe/device.h>
+
+/**
+ * @file
+ *
+ * Device model
+ *
+ */
+
+/** Registered root devices */
+static LIST_HEAD ( devices );
+
+/** Device removal inhibition counter */
+int device_keep_count = 0;
+
+/**
+ * Probe a root device
+ *
+ * @v rootdev		Root device
+ * @ret rc		Return status code
+ */
+static int rootdev_probe ( struct root_device *rootdev ) {
+	int rc;
+
+	DBG ( "Adding %s root bus\n", rootdev->dev.name );
+	if ( ( rc = rootdev->driver->probe ( rootdev ) ) != 0 ) {
+		DBG ( "Failed to add %s root bus: %s\n",
+		      rootdev->dev.name, strerror ( rc ) );
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * Remove a root device
+ *
+ * @v rootdev		Root device
+ */
+static void rootdev_remove ( struct root_device *rootdev ) {
+	rootdev->driver->remove ( rootdev );
+	DBG ( "Removed %s root bus\n", rootdev->dev.name );
+}
+
+/**
+ * Probe all devices
+ *
+ * This initiates probing for all devices in the system.  After this
+ * call, the device hierarchy will be populated, and all hardware
+ * should be ready to use.
+ */
+static void probe_devices ( void ) {
+	struct root_device *rootdev;
+	int rc;
+
+	for_each_table_entry ( rootdev, ROOT_DEVICES ) {
+		list_add ( &rootdev->dev.siblings, &devices );
+		INIT_LIST_HEAD ( &rootdev->dev.children );
+		if ( ( rc = rootdev_probe ( rootdev ) ) != 0 )
+			list_del ( &rootdev->dev.siblings );
+	}
+}
+
+/**
+ * Remove all devices
+ *
+ */
+static void remove_devices ( int booting __unused ) {
+	struct root_device *rootdev;
+	struct root_device *tmp;
+
+	if ( device_keep_count != 0 ) {
+		DBG ( "Refusing to remove devices on shutdown\n" );
+		return;
+	}
+
+	list_for_each_entry_safe ( rootdev, tmp, &devices, dev.siblings ) {
+		rootdev_remove ( rootdev );
+		list_del ( &rootdev->dev.siblings );
+	}
+}
+
+//struct startup_fn startup_devices __startup_fn ( STARTUP_NORMAL ) = {
+struct startup_fn startup_devices  = {
+	.name = "devices",
+	.startup = probe_devices,
+	.shutdown = remove_devices,
+};
+
+/**
+ * Identify a device behind an interface
+ *
+ * @v intf		Interface
+ * @ret device		Device, or NULL
+ */
+struct device * identify_device ( struct interface *intf ) {
+	struct interface *dest;
+	identify_device_TYPE ( void * ) *op =
+		intf_get_dest_op ( intf, identify_device, &dest );
+	void *object = intf_object ( dest );
+	void *device;
+
+	if ( op ) {
+		device = op ( object );
+	} else {
+		/* Default is to return NULL */
+		device = NULL;
+	}
+
+	intf_put ( dest );
+	return device;
+}

IPXE/ipxe-3fe683e/src/core/main.c

+/**************************************************************************
+iPXE -  Network Bootstrap Program
+
+Literature dealing with the network protocols:
+	ARP - RFC826
+	RARP - RFC903
+	UDP - RFC768
+	BOOTP - RFC951, RFC2132 (vendor extensions)
+	DHCP - RFC2131, RFC2132 (options)
+	TFTP - RFC1350, RFC2347 (options), RFC2348 (blocksize), RFC2349 (tsize)
+	RPC - RFC1831, RFC1832 (XDR), RFC1833 (rpcbind/portmapper)
+
+**************************************************************************/
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stddef.h>
+#include <stdio.h>
+#include <ipxe/init.h>
+#include <ipxe/version.h>
+#include <usr/autoboot.h>
+#include <ventoy.h>
+
+/**
+ * Main entry point
+ *
+ * @ret rc		Return status code
+ */
+__asmcall int main ( void ) {
+	int rc;
+
+	/* Perform one-time-only initialisation (e.g. heap) */
+	initialise();
+
+	/* Some devices take an unreasonably long time to initialise */
+	//printf ( "%s initialising devices...", product_short_name );
+	startup();
+	//printf ( "ok\n" );
+
+	/* Attempt to boot */
+	if ( ( rc = ventoy_boot_vdisk ( NULL ) ) != 0 )
+		goto err_ipxe;
+
+ err_ipxe:
+	shutdown_exit();
+	return rc;
+}

IPXE/ipxe-3fe683e/src/core/ventoy_dummy.c

+/*
+ * Copyright (C) 2017 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+/**
+ * @file
+ *
+ * SAN booting
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <assert.h>
+#include <ipxe/xfer.h>
+#include <ipxe/open.h>
+#include <ipxe/timer.h>
+#include <ipxe/process.h>
+#include <ipxe/iso9660.h>
+#include <ipxe/dhcp.h>
+#include <ipxe/settings.h>
+#include <ipxe/quiesce.h>
+#include <ipxe/sanboot.h>
+#include <ipxe/pci.h>
+#include <ipxe/scsi.h>
+#include <ipxe/ata.h>
+#include <ipxe/acpi.h>
+#include <ipxe/ibft.h>
+
+unsigned long pci_bar_size ( struct pci_device *pci, unsigned int reg )
+{
+    (void)pci;
+    (void)reg;
+    
+    return 0;
+}
+
+unsigned long pci_bar_start ( struct pci_device *pci, unsigned int reg )
+{
+    (void)pci;
+    (void)reg;
+    return 0;
+}
+
+void adjust_pci_device ( struct pci_device *pci )
+{
+    (void)pci;
+}
+
+/*
+ * obj_netfront
+ * obj_netvsc
+ * 
+ * ibft_model
+ */
+
+int scsi_parse_lun ( const char *lun_string, struct scsi_lun *lun )
+{
+    (void)lun_string;
+    (void)lun;
+    return 0;
+}
+
+void scsi_parse_sense ( const void *data, size_t len,
+			       struct scsi_sns_descriptor *sense )
+{
+    (void)data;
+    (void)len;
+    (void)sense;
+    
+    return;
+}
+
+void scsi_response ( struct interface *intf, struct scsi_rsp *response )
+{
+    (void)intf;
+    (void)response;
+}
+int scsi_open ( struct interface *block, struct interface *scsi,
+		       struct scsi_lun *lun )
+{
+    (void)block;
+    (void)scsi;
+    (void)lun;
+
+    return 0;
+}
+
+int scsi_command ( struct interface *control, struct interface *data,
+			  struct scsi_cmd *command )
+{
+    (void)control;
+    (void)data;
+    (void)command;
+    
+    return 0;
+}
+               
+
+int ata_open ( struct interface *block, struct interface *ata,
+		      unsigned int device, unsigned int max_count )
+{
+    (void)block;
+    (void)ata;
+    (void)device;
+    (void)max_count;
+    
+    return 0;
+}
+
+int ata_command ( struct interface *control, struct interface *data,
+			 struct ata_cmd *command )
+{
+    (void)control;
+    (void)data;
+    (void)command;
+
+    return 0;
+}
+
+#if 0
+static uint32_t mCrcTable[256] = 
+{
+    0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, 
+    0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91, 
+    0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, 
+    0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, 
+    0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B, 
+    0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, 
+    0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, 
+    0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D, 
+    0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433, 
+    0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, 
+    0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, 
+    0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65, 
+    0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, 
+    0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, 
+    0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F, 
+    0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, 
+    0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683, 
+    0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, 
+    0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, 
+    0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5, 
+    0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, 
+    0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, 
+    0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F, 
+    0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, 
+    0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713, 
+    0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, 
+    0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, 
+    0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45, 
+    0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, 
+    0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, 
+    0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF, 
+    0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
+};
+
+uint32_t CalculateCrc32
+(
+    const void       *Buffer,
+    uint32_t          Length,
+    uint32_t          InitValue
+)
+{
+    uint32_t Crc = 0;
+    uint32_t Index = 0;
+    const uint8_t *Ptr = (const uint8_t *)Buffer;
+
+    Crc = InitValue ^ 0xffffffff;
+    for (Index = 0; Index < Length; Index++, Ptr++)
+    {
+        Crc = (Crc >> 8) ^ mCrcTable[(uint8_t) Crc ^ *Ptr];
+    }
+
+    return (Crc ^ 0xffffffff);
+}
+
+#endif
+

IPXE/ipxe-3fe683e/src/core/vsprintf.c

+/*
+ * Copyright (C) 2006 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stddef.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <errno.h>
+#include <wchar.h>
+#include <ipxe/vsprintf.h>
+
+/** @file */
+
+#define CHAR_LEN	0	/**< "hh" length modifier */
+#define SHORT_LEN	1	/**< "h" length modifier */
+#define INT_LEN		2	/**< no length modifier */
+#define LONG_LEN	3	/**< "l" length modifier */
+#define LONGLONG_LEN	4	/**< "ll" length modifier */
+#define SIZE_T_LEN	5	/**< "z" length modifier */
+
+static uint8_t type_sizes[] = {
+	[CHAR_LEN]	= sizeof ( char ),
+	[SHORT_LEN]	= sizeof ( short ),
+	[INT_LEN]	= sizeof ( int ),
+	[LONG_LEN]	= sizeof ( long ),
+	[LONGLONG_LEN]	= sizeof ( long long ),
+	[SIZE_T_LEN]	= sizeof ( size_t ),
+};
+
+/**
+ * Use lower-case for hexadecimal digits
+ *
+ * Note that this value is set to 0x20 since that makes for very
+ * efficient calculations.  (Bitwise-ORing with @c LCASE converts to a
+ * lower-case character, for example.)
+ */
+#define LCASE 0x20
+
+/**
+ * Use "alternate form"
+ *
+ * For hexadecimal numbers, this means to add a "0x" or "0X" prefix to
+ * the number.
+ */
+#define ALT_FORM 0x02
+
+/**
+ * Use zero padding
+ *
+ * Note that this value is set to 0x10 since that allows the pad
+ * character to be calculated as @c 0x20|(flags&ZPAD)
+ */
+#define ZPAD 0x10
+
+/**
+ * Format a hexadecimal number
+ *
+ * @v end		End of buffer to contain number
+ * @v num		Number to format
+ * @v width		Minimum field width
+ * @v flags		Format flags
+ * @ret ptr		End of buffer
+ *
+ * Fills a buffer in reverse order with a formatted hexadecimal
+ * number.  The number will be zero-padded to the specified width.
+ * Lower-case and "alternate form" (i.e. "0x" prefix) flags may be
+ * set.
+ *
+ * There must be enough space in the buffer to contain the largest
+ * number that this function can format.
+ */
+static char * format_hex ( char *end, unsigned long long num, int width,
+			   int flags ) {
+	char *ptr = end;
+	int case_mod = ( flags & LCASE );
+	int pad = ( ( flags & ZPAD ) | ' ' );
+
+	/* Generate the number */
+	do {
+		*(--ptr) = "0123456789ABCDEF"[ num & 0xf ] | case_mod;
+		num >>= 4;
+	} while ( num );
+
+	/* Pad to width */
+	while ( ( end - ptr ) < width )
+		*(--ptr) = pad;
+
+	/* Add "0x" or "0X" if alternate form specified */
+	if ( flags & ALT_FORM ) {
+		*(--ptr) = 'X' | case_mod;
+		*(--ptr) = '0';
+	}
+
+	return ptr;
+}
+
+/**
+ * Format a decimal number
+ *
+ * @v end		End of buffer to contain number
+ * @v num		Number to format
+ * @v width		Minimum field width
+ * @v flags		Format flags
+ * @ret ptr		End of buffer
+ *
+ * Fills a buffer in reverse order with a formatted decimal number.
+ * The number will be space-padded to the specified width.
+ *
+ * There must be enough space in the buffer to contain the largest
+ * number that this function can format.
+ */
+static char * format_decimal ( char *end, signed long num, int width,
+			       int flags ) {
+	char *ptr = end;
+	int negative = 0;
+	int zpad = ( flags & ZPAD );
+	int pad = ( zpad | ' ' );
+
+	/* Generate the number */
+	if ( num < 0 ) {
+		negative = 1;
+		num = -num;
+	}
+	do {
+		*(--ptr) = '0' + ( num % 10 );
+		num /= 10;
+	} while ( num );
+
+	/* Add "-" if necessary */
+	if ( negative && ( ! zpad ) )
+		*(--ptr) = '-';
+
+	/* Pad to width */
+	while ( ( end - ptr ) < width )
+		*(--ptr) = pad;
+
+	/* Add "-" if necessary */
+	if ( negative && zpad )
+		*ptr = '-';
+
+	return ptr;
+}
+
+#define ZPAD 0x10
+char *format_unsigned_decimal(char *end, unsigned long long num, int width, int flags) 
+{
+	char *ptr = end;
+	int zpad = ( flags & ZPAD );
+	int pad = ( zpad | ' ' );
+
+	do {
+		*(--ptr) = '0' + ( num % 10 );
+		num /= 10;
+	} while ( num );
+
+	/* Pad to width */
+	while ( ( end - ptr ) < width )
+		*(--ptr) = pad;
+
+	return ptr;
+}
+
+/**
+ * Print character via a printf context
+ *
+ * @v ctx		Context
+ * @v c			Character
+ *
+ * Call's the printf_context::handler() method and increments
+ * printf_context::len.
+ */
+static inline void cputchar ( struct printf_context *ctx, unsigned char c ) {
+	ctx->handler ( ctx, c );
+	++ctx->len;
+}
+
+/**
+ * Write a formatted string to a printf context
+ *
+ * @v ctx		Context
+ * @v fmt		Format string
+ * @v args		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ */
+size_t vcprintf ( struct printf_context *ctx, const char *fmt, va_list args ) {
+	int flags;
+	int width;
+	uint8_t *length;
+	char *ptr;
+	char tmp_buf[32]; /* 32 is enough for all numerical formats.
+			   * Insane width fields could overflow this buffer. */
+	wchar_t *wptr;
+
+	/* Initialise context */
+	ctx->len = 0;
+
+	for ( ; *fmt ; fmt++ ) {
+		/* Pass through ordinary characters */
+		if ( *fmt != '%' ) {
+			cputchar ( ctx, *fmt );
+			continue;
+		}
+		fmt++;
+		/* Process flag characters */
+		flags = 0;
+		for ( ; ; fmt++ ) {
+			if ( *fmt == '#' ) {
+				flags |= ALT_FORM;
+			} else if ( *fmt == '0' ) {
+				flags |= ZPAD;
+			} else {
+				/* End of flag characters */
+				break;
+			}
+		}
+		/* Process field width */
+		width = 0;
+		for ( ; ; fmt++ ) {
+			if ( ( ( unsigned ) ( *fmt - '0' ) ) < 10 ) {
+				width = ( width * 10 ) + ( *fmt - '0' );
+			} else {
+				break;
+			}
+		}
+		/* We don't do floating point */
+		/* Process length modifier */
+		length = &type_sizes[INT_LEN];
+		for ( ; ; fmt++ ) {
+			if ( *fmt == 'h' ) {
+				length--;
+			} else if ( *fmt == 'l' ) {
+				length++;
+			} else if ( *fmt == 'z' ) {
+				length = &type_sizes[SIZE_T_LEN];
+			} else {
+				break;
+			}
+		}
+		/* Process conversion specifier */
+		ptr = tmp_buf + sizeof ( tmp_buf ) - 1;
+		*ptr = '\0';
+		wptr = NULL;
+		if ( *fmt == 'c' ) {
+			if ( length < &type_sizes[LONG_LEN] ) {
+				cputchar ( ctx, va_arg ( args, unsigned int ) );
+			} else {
+				wchar_t wc;
+				size_t len;
+
+				wc = va_arg ( args, wint_t );
+				len = wcrtomb ( tmp_buf, wc, NULL );
+				tmp_buf[len] = '\0';
+				ptr = tmp_buf;
+			}
+		} else if ( *fmt == 's' ) {
+			if ( length < &type_sizes[LONG_LEN] ) {
+				ptr = va_arg ( args, char * );
+				if ( ! ptr )
+					ptr = "<NULL>";
+			} else {
+				wptr = va_arg ( args, wchar_t * );
+				if ( ! wptr )
+					ptr = "<NULL>";
+			}
+		} else if ( *fmt == 'p' ) {
+			intptr_t ptrval;
+
+			ptrval = ( intptr_t ) va_arg ( args, void * );
+			ptr = format_hex ( ptr, ptrval, width, 
+					   ( ALT_FORM | LCASE ) );
+		} else if ( ( *fmt & ~0x20 ) == 'X' ) {
+			unsigned long long hex;
+
+			flags |= ( *fmt & 0x20 ); /* LCASE */
+			if ( *length >= sizeof ( unsigned long long ) ) {
+				hex = va_arg ( args, unsigned long long );
+			} else if ( *length >= sizeof ( unsigned long ) ) {
+				hex = va_arg ( args, unsigned long );
+			} else {
+				hex = va_arg ( args, unsigned int );
+			}
+			ptr = format_hex ( ptr, hex, width, flags );
+		} else if ( ( *fmt == 'd' ) || ( *fmt == 'i' ) ){
+			signed long decimal;
+
+			if ( *length >= sizeof ( signed long ) ) {
+				decimal = va_arg ( args, signed long );
+			} else {
+				decimal = va_arg ( args, signed int );
+			}
+			ptr = format_decimal ( ptr, decimal, width, flags );
+		} else if ( ( *fmt == 'u' ) || ( *fmt == 'U' )){
+			unsigned long long decimal;
+            if ( *length >= sizeof ( unsigned long long ) ) {
+				decimal = va_arg ( args, unsigned long long );
+			} else if ( *length >= sizeof ( unsigned long ) ) {
+				decimal = va_arg ( args, unsigned long );
+			} else {
+				decimal = va_arg ( args, unsigned int );
+			}
+			ptr = format_unsigned_decimal( ptr, decimal, width, flags );
+		} else {
+			*(--ptr) = *fmt;
+		}
+		/* Write out conversion result */
+		if ( wptr == NULL ) {
+			for ( ; *ptr ; ptr++ ) {
+				cputchar ( ctx, *ptr );
+			}
+		} else {
+			for ( ; *wptr ; wptr++ ) {
+				size_t len = wcrtomb ( tmp_buf, *wptr, NULL );
+				for ( ptr = tmp_buf ; len-- ; ptr++ ) {
+					cputchar ( ctx, *ptr );
+				}
+			}
+		}
+	}
+
+	return ctx->len;
+}
+
+/** Context used by vsnprintf() and friends */
+struct sputc_context {
+	struct printf_context ctx;
+	/** Buffer for formatted string (used by printf_sputc()) */
+	char *buf;
+	/** Buffer length (used by printf_sputc()) */
+	size_t max_len;	
+};
+
+/**
+ * Write character to buffer
+ *
+ * @v ctx		Context
+ * @v c			Character
+ */
+static void printf_sputc ( struct printf_context *ctx, unsigned int c ) {
+	struct sputc_context * sctx =
+		container_of ( ctx, struct sputc_context, ctx );
+
+	if ( ctx->len < sctx->max_len )
+		sctx->buf[ctx->len] = c;
+}
+
+/**
+ * Write a formatted string to a buffer
+ *
+ * @v buf		Buffer into which to write the string
+ * @v size		Size of buffer
+ * @v fmt		Format string
+ * @v args		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ *
+ * If the buffer is too small to contain the string, the returned
+ * length is the length that would have been written had enough space
+ * been available.
+ */
+int vsnprintf ( char *buf, size_t size, const char *fmt, va_list args ) {
+	struct sputc_context sctx;
+	size_t len;
+	size_t end;
+
+	/* Hand off to vcprintf */
+	sctx.ctx.handler = printf_sputc;
+	sctx.buf = buf;
+	sctx.max_len = size;
+	len = vcprintf ( &sctx.ctx, fmt, args );
+
+	/* Add trailing NUL */
+	if ( size ) {
+		end = size - 1;
+		if ( len < end )
+			end = len;
+		buf[end] = '\0';
+	}
+
+	return len;
+}
+
+/**
+ * Write a formatted string to a buffer
+ *
+ * @v buf		Buffer into which to write the string
+ * @v size		Size of buffer
+ * @v fmt		Format string
+ * @v ...		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ */
+int snprintf ( char *buf, size_t size, const char *fmt, ... ) {
+	va_list args;
+	int i;
+
+	va_start ( args, fmt );
+	i = vsnprintf ( buf, size, fmt, args );
+	va_end ( args );
+	return i;
+}
+
+/**
+ * Version of vsnprintf() that accepts a signed buffer size
+ *
+ * @v buf		Buffer into which to write the string
+ * @v size		Size of buffer
+ * @v fmt		Format string
+ * @v args		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ */
+int vssnprintf ( char *buf, ssize_t ssize, const char *fmt, va_list args ) {
+
+	/* Treat negative buffer size as zero buffer size */
+	if ( ssize < 0 )
+		ssize = 0;
+
+	/* Hand off to vsnprintf */
+	return vsnprintf ( buf, ssize, fmt, args );
+}
+
+/**
+ * Version of vsnprintf() that accepts a signed buffer size
+ *
+ * @v buf		Buffer into which to write the string
+ * @v size		Size of buffer
+ * @v fmt		Format string
+ * @v ...		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ */
+int ssnprintf ( char *buf, ssize_t ssize, const char *fmt, ... ) {
+	va_list args;
+	int len;
+
+	/* Hand off to vssnprintf */
+	va_start ( args, fmt );
+	len = vssnprintf ( buf, ssize, fmt, args );
+	va_end ( args );
+	return len;
+}
+
+/**
+ * Write character to console
+ *
+ * @v ctx		Context
+ * @v c			Character
+ */
+static void printf_putchar ( struct printf_context *ctx __unused,
+			     unsigned int c ) {
+	putchar ( c );
+}
+
+/**
+ * Write a formatted string to the console
+ *
+ * @v fmt		Format string
+ * @v args		Arguments corresponding to the format string
+ * @ret len		Length of formatted string
+ */
+int vprintf ( const char *fmt, va_list args ) {
+	struct printf_context ctx;
+
+	/* Hand off to vcprintf */
+	ctx.handler = printf_putchar;	
+	return vcprintf ( &ctx, fmt, args );	
+}
+
+/**
+ * Write a formatted string to the console.
+ *
+ * @v fmt		Format string
+ * @v ...		Arguments corresponding to the format string
+ * @ret	len		Length of formatted string
+ */
+int printf ( const char *fmt, ... ) {
+	va_list args;
+	int i;
+
+	va_start ( args, fmt );
+	i = vprintf ( fmt, args );
+	va_end ( args );
+	return i;
+}

IPXE/ipxe-3fe683e/src/drivers/net/efi/snp.c

+/*
+ * Copyright (C) 2014 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#if 0
+
+#include <errno.h>
+#include <ipxe/efi/efi.h>
+#include <ipxe/efi/efi_driver.h>
+#include <ipxe/efi/efi_snp.h>
+#include "snpnet.h"
+#include "nii.h"
+
+/** @file
+ *
+ * SNP driver
+ *
+ */
+
+/**
+ * Check to see if driver supports a device
+ *
+ * @v device		EFI device handle
+ * @ret rc		Return status code
+ */
+static int snp_supported ( EFI_HANDLE device ) {
+	EFI_BOOT_SERVICES *bs = efi_systab->BootServices;
+	EFI_STATUS efirc;
+
+	/* Check that this is not a device we are providing ourselves */
+	if ( find_snpdev ( device ) != NULL ) {
+		DBGCP ( device, "SNP %s is provided by this binary\n",
+			efi_handle_name ( device ) );
+		return -ENOTTY;
+	}
+
+	/* Test for presence of simple network protocol */
+	if ( ( efirc = bs->OpenProtocol ( device,
+					  &efi_simple_network_protocol_guid,
+					  NULL, efi_image_handle, device,
+					  EFI_OPEN_PROTOCOL_TEST_PROTOCOL))!=0){
+		DBGCP ( device, "SNP %s is not an SNP device\n",
+			efi_handle_name ( device ) );
+		return -EEFI ( efirc );
+	}
+	DBGC ( device, "SNP %s is an SNP device\n",
+	       efi_handle_name ( device ) );
+
+	return 0;
+}
+
+/**
+ * Check to see if driver supports a device
+ *
+ * @v device		EFI device handle
+ * @ret rc		Return status code
+ */
+static int nii_supported ( EFI_HANDLE device ) {
+	EFI_BOOT_SERVICES *bs = efi_systab->BootServices;
+	EFI_STATUS efirc;
+
+	/* Check that this is not a device we are providing ourselves */
+	if ( find_snpdev ( device ) != NULL ) {
+		DBGCP ( device, "NII %s is provided by this binary\n",
+			efi_handle_name ( device ) );
+		return -ENOTTY;
+	}
+
+	/* Test for presence of NII protocol */
+	if ( ( efirc = bs->OpenProtocol ( device,
+					  &efi_nii31_protocol_guid,
+					  NULL, efi_image_handle, device,
+					  EFI_OPEN_PROTOCOL_TEST_PROTOCOL))!=0){
+		DBGCP ( device, "NII %s is not an NII device\n",
+			efi_handle_name ( device ) );
+		return -EEFI ( efirc );
+	}
+	DBGC ( device, "NII %s is an NII device\n",
+	       efi_handle_name ( device ) );
+
+	return 0;
+}
+
+/** EFI SNP driver */
+struct efi_driver snp_driver __efi_driver ( EFI_DRIVER_NORMAL ) = {
+	.name = "SNP",
+	.supported = snp_supported,
+	.start = snpnet_start,
+	.stop = snpnet_stop,
+};
+
+/** EFI NII driver */
+struct efi_driver nii_driver __efi_driver ( EFI_DRIVER_NORMAL ) = {
+	.name = "NII",
+	.supported = nii_supported,
+	.start = nii_start,
+	.stop = nii_stop,
+};
+
+#endif
+