diff mbox

[Xen-devel,V3,07/15] Move x86 specific video and disk probing code

Message ID 1410148441-18684-8-git-send-email-roy.franz@linaro.org
State New
Headers show

Commit Message

Roy Franz Sept. 8, 2014, 3:53 a.m. UTC 
Move x86 specific video (VGA) and disk (EDD) probing to arch specific
file.  This code is x86 only, so move it to arch specific code.  The
efi_arch_edd() and efi_arch_video() will be empty for ARM.

Signed-off-by: Roy Franz <roy.franz@linaro.org>
---
 xen/common/efi/boot.c          | 284 +--------------------------------------
 xen/include/asm-x86/efi-boot.h | 298 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 301 insertions(+), 281 deletions(-)
diff mbox

Patch

diff --git a/xen/common/efi/boot.c b/xen/common/efi/boot.c
index fd9d382..3fe04fd 100644
--- a/xen/common/efi/boot.c
+++ b/xen/common/efi/boot.c
@@ -543,34 +543,17 @@  static void __init split_value(char *s)
     *s = 0;
 }
 
-static int __init set_color(u32 mask, int bpp, u8 *pos, u8 *sz)
-{
-   if ( bpp < 0 )
-       return bpp;
-   if ( !mask )
-       return -EINVAL;
-   for ( *pos = 0; !(mask & 1); ++*pos )
-       mask >>= 1;
-   for ( *sz = 0; mask & 1; ++sz)
-       mask >>= 1;
-   if ( mask )
-       return -EINVAL;
-   return max(*pos + *sz, bpp);
-}
-
 void EFIAPI __init noreturn
 efi_start(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable)
 {
     static EFI_GUID __initdata loaded_image_guid = LOADED_IMAGE_PROTOCOL;
     static EFI_GUID __initdata gop_guid = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID;
-    static EFI_GUID __initdata bio_guid = BLOCK_IO_PROTOCOL;
-    static EFI_GUID __initdata devp_guid = DEVICE_PATH_PROTOCOL;
     static EFI_GUID __initdata shim_lock_guid = SHIM_LOCK_PROTOCOL_GUID;
     EFI_LOADED_IMAGE *loaded_image;
     EFI_STATUS status;
     unsigned int i, argc;
     CHAR16 **argv, *file_name, *cfg_file_name = NULL, *options = NULL;
-    UINTN cols, rows, depth, size, info_size, gop_mode = ~0;
+    UINTN cols, rows, depth, size, info_size;
     EFI_HANDLE *handles = NULL;
     EFI_SHIM_LOCK_PROTOCOL *shim_lock;
     EFI_GRAPHICS_OUTPUT_PROTOCOL *gop = NULL;
@@ -677,15 +660,6 @@  efi_start(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable)
 
     efi_arch_relocate_image(0);
 
-    if ( StdOut->QueryMode(StdOut, StdOut->Mode->Mode,
-                           &cols, &rows) == EFI_SUCCESS )
-    {
-        vga_console_info.video_type = XEN_VGATYPE_TEXT_MODE_3;
-        vga_console_info.u.text_mode_3.columns = cols;
-        vga_console_info.u.text_mode_3.rows = rows;
-        vga_console_info.u.text_mode_3.font_height = 16;
-    }
-
     size = 0;
     status = efi_bs->LocateHandle(ByProtocol, &gop_guid, NULL, &size, NULL);
     if ( status == EFI_BUFFER_TOO_SMALL )
@@ -811,189 +785,7 @@  efi_start(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable)
     cfg.addr = 0;
 
     dir_handle->Close(dir_handle);
-
-    if ( gop && !base_video )
-    {
-        for ( i = size = 0; i < gop->Mode->MaxMode; ++i )
-        {
-            unsigned int bpp = 0;
-
-            status = gop->QueryMode(gop, i, &info_size, &mode_info);
-            if ( EFI_ERROR(status) )
-                continue;
-            switch ( mode_info->PixelFormat )
-            {
-            case PixelBitMask:
-                bpp = hweight32(mode_info->PixelInformation.RedMask |
-                                mode_info->PixelInformation.GreenMask |
-                                mode_info->PixelInformation.BlueMask);
-                break;
-            case PixelRedGreenBlueReserved8BitPerColor:
-            case PixelBlueGreenRedReserved8BitPerColor:
-                bpp = 24;
-                break;
-            default:
-                continue;
-            }
-            if ( cols == mode_info->HorizontalResolution &&
-                 rows == mode_info->VerticalResolution &&
-                 (!depth || bpp == depth) )
-            {
-                gop_mode = i;
-                break;
-            }
-            if ( !cols && !rows &&
-                 mode_info->HorizontalResolution *
-                 mode_info->VerticalResolution > size )
-            {
-                size = mode_info->HorizontalResolution *
-                       mode_info->VerticalResolution;
-                gop_mode = i;
-            }
-        }
-    }
-
-    /* Collect EDD info. */
-    BUILD_BUG_ON(offsetof(struct edd_info, edd_device_params) != EDDEXTSIZE);
-    BUILD_BUG_ON(sizeof(struct edd_device_params) != EDDPARMSIZE);
-    size = 0;
-    status = efi_bs->LocateHandle(ByProtocol, &bio_guid, NULL, &size, NULL);
-    if ( status == EFI_BUFFER_TOO_SMALL )
-        status = efi_bs->AllocatePool(EfiLoaderData, size, (void **)&handles);
-    if ( !EFI_ERROR(status) )
-        status = efi_bs->LocateHandle(ByProtocol, &bio_guid, NULL, &size,
-                                      handles);
-    if ( EFI_ERROR(status) )
-        size = 0;
-    for ( i = 0; i < size / sizeof(*handles); ++i )
-    {
-        EFI_BLOCK_IO *bio;
-        EFI_DEV_PATH_PTR devp;
-        struct edd_info *info = boot_edd_info + boot_edd_info_nr;
-        struct edd_device_params *params = &info->edd_device_params;
-        enum { root, acpi, pci, ctrlr } state = root;
-
-        status = efi_bs->HandleProtocol(handles[i], &bio_guid, (void **)&bio);
-        if ( EFI_ERROR(status) ||
-             bio->Media->RemovableMedia ||
-             bio->Media->LogicalPartition )
-            continue;
-        if ( boot_edd_info_nr < EDD_INFO_MAX )
-        {
-            info->device = 0x80 + boot_edd_info_nr; /* fake */
-            info->version = 0x11;
-            params->length = offsetof(struct edd_device_params, dpte_ptr);
-            params->number_of_sectors = bio->Media->LastBlock + 1;
-            params->bytes_per_sector = bio->Media->BlockSize;
-            params->dpte_ptr = ~0;
-        }
-        ++boot_edd_info_nr;
-        status = efi_bs->HandleProtocol(handles[i], &devp_guid,
-                                        (void **)&devp);
-        if ( EFI_ERROR(status) )
-            continue;
-        for ( ; !IsDevicePathEnd(devp.DevPath);
-              devp.DevPath = NextDevicePathNode(devp.DevPath) )
-        {
-            switch ( DevicePathType(devp.DevPath) )
-            {
-                const u8 *p;
-
-            case ACPI_DEVICE_PATH:
-                if ( state != root || boot_edd_info_nr > EDD_INFO_MAX )
-                    break;
-                switch ( DevicePathSubType(devp.DevPath) )
-                {
-                case ACPI_DP:
-                    if ( devp.Acpi->HID != EISA_PNP_ID(0xA03) &&
-                         devp.Acpi->HID != EISA_PNP_ID(0xA08) )
-                        break;
-                    params->interface_path.pci.bus = devp.Acpi->UID;
-                    state = acpi;
-                    break;
-                case EXPANDED_ACPI_DP:
-                    /* XXX */
-                    break;
-                }
-                break;
-            case HARDWARE_DEVICE_PATH:
-                if ( state != acpi ||
-                     DevicePathSubType(devp.DevPath) != HW_PCI_DP ||
-                     boot_edd_info_nr > EDD_INFO_MAX )
-                    break;
-                state = pci;
-                edd_put_string(params->host_bus_type, "PCI");
-                params->interface_path.pci.slot = devp.Pci->Device;
-                params->interface_path.pci.function = devp.Pci->Function;
-                break;
-            case MESSAGING_DEVICE_PATH:
-                if ( state != pci || boot_edd_info_nr > EDD_INFO_MAX )
-                    break;
-                state = ctrlr;
-                switch ( DevicePathSubType(devp.DevPath) )
-                {
-                case MSG_ATAPI_DP:
-                    edd_put_string(params->interface_type, "ATAPI");
-                    params->interface_path.pci.channel =
-                        devp.Atapi->PrimarySecondary;
-                    params->device_path.atapi.device = devp.Atapi->SlaveMaster;
-                    params->device_path.atapi.lun = devp.Atapi->Lun;
-                    break;
-                case MSG_SCSI_DP:
-                    edd_put_string(params->interface_type, "SCSI");
-                    params->device_path.scsi.id = devp.Scsi->Pun;
-                    params->device_path.scsi.lun = devp.Scsi->Lun;
-                    break;
-                case MSG_FIBRECHANNEL_DP:
-                    edd_put_string(params->interface_type, "FIBRE");
-                    params->device_path.fibre.wwid = devp.FibreChannel->WWN;
-                    params->device_path.fibre.lun = devp.FibreChannel->Lun;
-                    break;
-                case MSG_1394_DP:
-                    edd_put_string(params->interface_type, "1394");
-                    params->device_path.i1394.eui = devp.F1394->Guid;
-                    break;
-                case MSG_USB_DP:
-                case MSG_USB_CLASS_DP:
-                    edd_put_string(params->interface_type, "USB");
-                    break;
-                case MSG_I2O_DP:
-                    edd_put_string(params->interface_type, "I2O");
-                    params->device_path.i2o.identity_tag = devp.I2O->Tid;
-                    break;
-                default:
-                    continue;
-                }
-                info->version = 0x30;
-                params->length = sizeof(struct edd_device_params);
-                params->key = 0xbedd;
-                params->device_path_info_length =
-                    sizeof(struct edd_device_params) -
-                    offsetof(struct edd_device_params, key);
-                for ( p = (const u8 *)&params->key; p < &params->checksum; ++p )
-                    params->checksum -= *p;
-                break;
-            case MEDIA_DEVICE_PATH:
-                if ( DevicePathSubType(devp.DevPath) == MEDIA_HARDDRIVE_DP &&
-                     devp.HardDrive->MBRType == MBR_TYPE_PCAT &&
-                     boot_mbr_signature_nr < EDD_MBR_SIG_MAX )
-                {
-                    struct mbr_signature *sig = boot_mbr_signature +
-                                                boot_mbr_signature_nr;
-
-                    sig->device = 0x80 + boot_edd_info_nr; /* fake */
-                    memcpy(&sig->signature, devp.HardDrive->Signature,
-                           sizeof(sig->signature));
-                    ++boot_mbr_signature_nr;
-                }
-                break;
-            }
-        }
-    }
-    if ( handles )
-        efi_bs->FreePool(handles);
-    if ( boot_edd_info_nr > EDD_INFO_MAX )
-        boot_edd_info_nr = EDD_INFO_MAX;
+    efi_arch_edd();
 
     /* XXX Collect EDID info. */
 
@@ -1075,77 +867,7 @@  efi_start(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable)
     l3_bootmap[l3_table_offset(xen_phys_start + (8 << L2_PAGETABLE_SHIFT) - 1)] =
         l3e_from_paddr((UINTN)l2_bootmap, __PAGE_HYPERVISOR);
 
-    if ( gop )
-    {
-        int bpp = 0;
-
-        /* Set graphics mode. */
-        if ( gop_mode < gop->Mode->MaxMode && gop_mode != gop->Mode->Mode )
-            gop->SetMode(gop, gop_mode);
-
-        /* Get graphics and frame buffer info. */
-        status = gop->QueryMode(gop, gop->Mode->Mode, &info_size, &mode_info);
-        if ( !EFI_ERROR(status) )
-            switch ( mode_info->PixelFormat )
-            {
-            case PixelRedGreenBlueReserved8BitPerColor:
-                vga_console_info.u.vesa_lfb.red_pos = 0;
-                vga_console_info.u.vesa_lfb.red_size = 8;
-                vga_console_info.u.vesa_lfb.green_pos = 8;
-                vga_console_info.u.vesa_lfb.green_size = 8;
-                vga_console_info.u.vesa_lfb.blue_pos = 16;
-                vga_console_info.u.vesa_lfb.blue_size = 8;
-                vga_console_info.u.vesa_lfb.rsvd_pos = 24;
-                vga_console_info.u.vesa_lfb.rsvd_size = 8;
-                bpp = 32;
-                break;
-            case PixelBlueGreenRedReserved8BitPerColor:
-                vga_console_info.u.vesa_lfb.red_pos = 16;
-                vga_console_info.u.vesa_lfb.red_size = 8;
-                vga_console_info.u.vesa_lfb.green_pos = 8;
-                vga_console_info.u.vesa_lfb.green_size = 8;
-                vga_console_info.u.vesa_lfb.blue_pos = 0;
-                vga_console_info.u.vesa_lfb.blue_size = 8;
-                vga_console_info.u.vesa_lfb.rsvd_pos = 24;
-                vga_console_info.u.vesa_lfb.rsvd_size = 8;
-                bpp = 32;
-                break;
-            case PixelBitMask:
-                bpp = set_color(mode_info->PixelInformation.RedMask, bpp,
-                                &vga_console_info.u.vesa_lfb.red_pos,
-                                &vga_console_info.u.vesa_lfb.red_size);
-                bpp = set_color(mode_info->PixelInformation.GreenMask, bpp,
-                                &vga_console_info.u.vesa_lfb.green_pos,
-                                &vga_console_info.u.vesa_lfb.green_size);
-                bpp = set_color(mode_info->PixelInformation.BlueMask, bpp,
-                                &vga_console_info.u.vesa_lfb.blue_pos,
-                                &vga_console_info.u.vesa_lfb.blue_size);
-                bpp = set_color(mode_info->PixelInformation.ReservedMask, bpp,
-                                &vga_console_info.u.vesa_lfb.rsvd_pos,
-                                &vga_console_info.u.vesa_lfb.rsvd_size);
-                if ( bpp > 0 )
-                    break;
-                /* fall through */
-            default:
-                PrintErr(L"Current graphics mode is unsupported!\r\n");
-                status = EFI_UNSUPPORTED;
-                break;
-            }
-        if ( !EFI_ERROR(status) )
-        {
-            vga_console_info.video_type = XEN_VGATYPE_EFI_LFB;
-            vga_console_info.u.vesa_lfb.gbl_caps = 2; /* possibly non-VGA */
-            vga_console_info.u.vesa_lfb.width =
-                mode_info->HorizontalResolution;
-            vga_console_info.u.vesa_lfb.height = mode_info->VerticalResolution;
-            vga_console_info.u.vesa_lfb.bits_per_pixel = bpp;
-            vga_console_info.u.vesa_lfb.bytes_per_line =
-                (mode_info->PixelsPerScanLine * bpp + 7) >> 3;
-            vga_console_info.u.vesa_lfb.lfb_base = gop->Mode->FrameBufferBase;
-            vga_console_info.u.vesa_lfb.lfb_size =
-                (gop->Mode->FrameBufferSize + 0xffff) >> 16;
-        }
-    }
+    efi_arch_video(base_video, cols, rows, depth, gop);
 
     efi_arch_get_memory_map(&mmap_size, &mmap, &mmap_key,
                                   &mmap_desc_size, &mmap_desc_ver);
diff --git a/xen/include/asm-x86/efi-boot.h b/xen/include/asm-x86/efi-boot.h
index dea9be3..711ab12 100644
--- a/xen/include/asm-x86/efi-boot.h
+++ b/xen/include/asm-x86/efi-boot.h
@@ -639,3 +639,301 @@  static void __init efi_arch_handle_cmdline(CHAR16 *image_name,
     mbi.boot_loader_name = (long)"EFI";
     mbi.mods_addr = (long)mb_modules;
 }
+static void __init efi_arch_edd(void)
+{
+    static EFI_GUID __initdata bio_guid = BLOCK_IO_PROTOCOL;
+    static EFI_GUID __initdata devp_guid = DEVICE_PATH_PROTOCOL;
+    EFI_HANDLE *handles = NULL;
+    int i;
+    UINTN size;
+    EFI_STATUS status;
+
+    /* Collect EDD info. */
+    BUILD_BUG_ON(offsetof(struct edd_info, edd_device_params) != EDDEXTSIZE);
+    BUILD_BUG_ON(sizeof(struct edd_device_params) != EDDPARMSIZE);
+    size = 0;
+    status = efi_bs->LocateHandle(ByProtocol, &bio_guid, NULL, &size, NULL);
+    if ( status == EFI_BUFFER_TOO_SMALL )
+        status = efi_bs->AllocatePool(EfiLoaderData, size, (void **)&handles);
+    if ( !EFI_ERROR(status) )
+        status = efi_bs->LocateHandle(ByProtocol, &bio_guid, NULL, &size,
+                                      handles);
+    if ( EFI_ERROR(status) )
+        size = 0;
+    for ( i = 0; i < size / sizeof(*handles); ++i )
+    {
+        EFI_BLOCK_IO *bio;
+        EFI_DEV_PATH_PTR devp;
+        struct edd_info *info = boot_edd_info + boot_edd_info_nr;
+        struct edd_device_params *params = &info->edd_device_params;
+        enum { root, acpi, pci, ctrlr } state = root;
+
+        status = efi_bs->HandleProtocol(handles[i], &bio_guid, (void **)&bio);
+        if ( EFI_ERROR(status) ||
+             bio->Media->RemovableMedia ||
+             bio->Media->LogicalPartition )
+            continue;
+        if ( boot_edd_info_nr < EDD_INFO_MAX )
+        {
+            info->device = 0x80 + boot_edd_info_nr; /* fake */
+            info->version = 0x11;
+            params->length = offsetof(struct edd_device_params, dpte_ptr);
+            params->number_of_sectors = bio->Media->LastBlock + 1;
+            params->bytes_per_sector = bio->Media->BlockSize;
+            params->dpte_ptr = ~0;
+        }
+        ++boot_edd_info_nr;
+        status = efi_bs->HandleProtocol(handles[i], &devp_guid,
+                                        (void **)&devp);
+        if ( EFI_ERROR(status) )
+            continue;
+        for ( ; !IsDevicePathEnd(devp.DevPath);
+              devp.DevPath = NextDevicePathNode(devp.DevPath) )
+        {
+            switch ( DevicePathType(devp.DevPath) )
+            {
+                const u8 *p;
+
+            case ACPI_DEVICE_PATH:
+                if ( state != root || boot_edd_info_nr > EDD_INFO_MAX )
+                    break;
+                switch ( DevicePathSubType(devp.DevPath) )
+                {
+                case ACPI_DP:
+                    if ( devp.Acpi->HID != EISA_PNP_ID(0xA03) &&
+                         devp.Acpi->HID != EISA_PNP_ID(0xA08) )
+                        break;
+                    params->interface_path.pci.bus = devp.Acpi->UID;
+                    state = acpi;
+                    break;
+                case EXPANDED_ACPI_DP:
+                    /* XXX */
+                    break;
+                }
+                break;
+            case HARDWARE_DEVICE_PATH:
+                if ( state != acpi ||
+                     DevicePathSubType(devp.DevPath) != HW_PCI_DP ||
+                     boot_edd_info_nr > EDD_INFO_MAX )
+                    break;
+                state = pci;
+                edd_put_string(params->host_bus_type, "PCI");
+                params->interface_path.pci.slot = devp.Pci->Device;
+                params->interface_path.pci.function = devp.Pci->Function;
+                break;
+            case MESSAGING_DEVICE_PATH:
+                if ( state != pci || boot_edd_info_nr > EDD_INFO_MAX )
+                    break;
+                state = ctrlr;
+                switch ( DevicePathSubType(devp.DevPath) )
+                {
+                case MSG_ATAPI_DP:
+                    edd_put_string(params->interface_type, "ATAPI");
+                    params->interface_path.pci.channel =
+                        devp.Atapi->PrimarySecondary;
+                    params->device_path.atapi.device = devp.Atapi->SlaveMaster;
+                    params->device_path.atapi.lun = devp.Atapi->Lun;
+                    break;
+                case MSG_SCSI_DP:
+                    edd_put_string(params->interface_type, "SCSI");
+                    params->device_path.scsi.id = devp.Scsi->Pun;
+                    params->device_path.scsi.lun = devp.Scsi->Lun;
+                    break;
+                case MSG_FIBRECHANNEL_DP:
+                    edd_put_string(params->interface_type, "FIBRE");
+                    params->device_path.fibre.wwid = devp.FibreChannel->WWN;
+                    params->device_path.fibre.lun = devp.FibreChannel->Lun;
+                    break;
+                case MSG_1394_DP:
+                    edd_put_string(params->interface_type, "1394");
+                    params->device_path.i1394.eui = devp.F1394->Guid;
+                    break;
+                case MSG_USB_DP:
+                case MSG_USB_CLASS_DP:
+                    edd_put_string(params->interface_type, "USB");
+                    break;
+                case MSG_I2O_DP:
+                    edd_put_string(params->interface_type, "I2O");
+                    params->device_path.i2o.identity_tag = devp.I2O->Tid;
+                    break;
+                default:
+                    continue;
+                }
+                info->version = 0x30;
+                params->length = sizeof(struct edd_device_params);
+                params->key = 0xbedd;
+                params->device_path_info_length =
+                    sizeof(struct edd_device_params) -
+                    offsetof(struct edd_device_params, key);
+                for ( p = (const u8 *)&params->key; p < &params->checksum; ++p )
+                    params->checksum -= *p;
+                break;
+            case MEDIA_DEVICE_PATH:
+                if ( DevicePathSubType(devp.DevPath) == MEDIA_HARDDRIVE_DP &&
+                     devp.HardDrive->MBRType == MBR_TYPE_PCAT &&
+                     boot_mbr_signature_nr < EDD_MBR_SIG_MAX )
+                {
+                    struct mbr_signature *sig = boot_mbr_signature +
+                                                boot_mbr_signature_nr;
+
+                    sig->device = 0x80 + boot_edd_info_nr; /* fake */
+                    memcpy(&sig->signature, devp.HardDrive->Signature,
+                           sizeof(sig->signature));
+                    ++boot_mbr_signature_nr;
+                }
+                break;
+            }
+        }
+    }
+    if ( handles )
+        efi_bs->FreePool(handles);
+    if ( boot_edd_info_nr > EDD_INFO_MAX )
+        boot_edd_info_nr = EDD_INFO_MAX;
+}
+
+static int __init set_color(u32 mask, int bpp, u8 *pos, u8 *sz)
+{
+   if ( bpp < 0 )
+       return bpp;
+   if ( !mask )
+       return -EINVAL;
+   for ( *pos = 0; !(mask & 1); ++*pos )
+       mask >>= 1;
+   for ( *sz = 0; mask & 1; ++sz)
+       mask >>= 1;
+   if ( mask )
+       return -EINVAL;
+   return max(*pos + *sz, bpp);
+}
+
+static void __init efi_arch_video(bool_t base_video,
+                                  UINTN cols, UINTN rows, UINTN depth,
+                                  EFI_GRAPHICS_OUTPUT_PROTOCOL *gop)
+{
+    int i;
+    EFI_STATUS status;
+    UINTN size, info_size, gop_mode = ~0;
+    EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *mode_info;
+
+    if ( gop && !base_video )
+    {
+        for ( i = size = 0; i < gop->Mode->MaxMode; ++i )
+        {
+            unsigned int bpp = 0;
+
+            status = gop->QueryMode(gop, i, &info_size, &mode_info);
+            if ( EFI_ERROR(status) )
+                continue;
+            switch ( mode_info->PixelFormat )
+            {
+            case PixelBitMask:
+                bpp = hweight32(mode_info->PixelInformation.RedMask |
+                                mode_info->PixelInformation.GreenMask |
+                                mode_info->PixelInformation.BlueMask);
+                break;
+            case PixelRedGreenBlueReserved8BitPerColor:
+            case PixelBlueGreenRedReserved8BitPerColor:
+                bpp = 24;
+                break;
+            default:
+                continue;
+            }
+            if ( cols == mode_info->HorizontalResolution &&
+                 rows == mode_info->VerticalResolution &&
+                 (!depth || bpp == depth) )
+            {
+                gop_mode = i;
+                break;
+            }
+            if ( !cols && !rows &&
+                 mode_info->HorizontalResolution *
+                 mode_info->VerticalResolution > size )
+            {
+                size = mode_info->HorizontalResolution *
+                       mode_info->VerticalResolution;
+                gop_mode = i;
+            }
+        }
+    }
+
+    if ( StdOut->QueryMode(StdOut, StdOut->Mode->Mode,
+                           &cols, &rows) == EFI_SUCCESS )
+    {
+        vga_console_info.video_type = XEN_VGATYPE_TEXT_MODE_3;
+        vga_console_info.u.text_mode_3.columns = cols;
+        vga_console_info.u.text_mode_3.rows = rows;
+        vga_console_info.u.text_mode_3.font_height = 16;
+    }
+
+    if ( gop )
+    {
+        int bpp = 0;
+
+        /* Set graphics mode. */
+        if ( gop_mode < gop->Mode->MaxMode && gop_mode != gop->Mode->Mode )
+            gop->SetMode(gop, gop_mode);
+
+        /* Get graphics and frame buffer info. */
+        status = gop->QueryMode(gop, gop->Mode->Mode, &info_size, &mode_info);
+        if ( !EFI_ERROR(status) )
+            switch ( mode_info->PixelFormat )
+            {
+            case PixelRedGreenBlueReserved8BitPerColor:
+                vga_console_info.u.vesa_lfb.red_pos = 0;
+                vga_console_info.u.vesa_lfb.red_size = 8;
+                vga_console_info.u.vesa_lfb.green_pos = 8;
+                vga_console_info.u.vesa_lfb.green_size = 8;
+                vga_console_info.u.vesa_lfb.blue_pos = 16;
+                vga_console_info.u.vesa_lfb.blue_size = 8;
+                vga_console_info.u.vesa_lfb.rsvd_pos = 24;
+                vga_console_info.u.vesa_lfb.rsvd_size = 8;
+                bpp = 32;
+                break;
+            case PixelBlueGreenRedReserved8BitPerColor:
+                vga_console_info.u.vesa_lfb.red_pos = 16;
+                vga_console_info.u.vesa_lfb.red_size = 8;
+                vga_console_info.u.vesa_lfb.green_pos = 8;
+                vga_console_info.u.vesa_lfb.green_size = 8;
+                vga_console_info.u.vesa_lfb.blue_pos = 0;
+                vga_console_info.u.vesa_lfb.blue_size = 8;
+                vga_console_info.u.vesa_lfb.rsvd_pos = 24;
+                vga_console_info.u.vesa_lfb.rsvd_size = 8;
+                bpp = 32;
+                break;
+            case PixelBitMask:
+                bpp = set_color(mode_info->PixelInformation.RedMask, bpp,
+                                &vga_console_info.u.vesa_lfb.red_pos,
+                                &vga_console_info.u.vesa_lfb.red_size);
+                bpp = set_color(mode_info->PixelInformation.GreenMask, bpp,
+                                &vga_console_info.u.vesa_lfb.green_pos,
+                                &vga_console_info.u.vesa_lfb.green_size);
+                bpp = set_color(mode_info->PixelInformation.BlueMask, bpp,
+                                &vga_console_info.u.vesa_lfb.blue_pos,
+                                &vga_console_info.u.vesa_lfb.blue_size);
+                bpp = set_color(mode_info->PixelInformation.ReservedMask, bpp,
+                                &vga_console_info.u.vesa_lfb.rsvd_pos,
+                                &vga_console_info.u.vesa_lfb.rsvd_size);
+                if ( bpp > 0 )
+                    break;
+                /* fall through */
+            default:
+                PrintErr(L"Current graphics mode is unsupported!\r\n");
+                status = EFI_UNSUPPORTED;
+                break;
+            }
+        if ( !EFI_ERROR(status) )
+        {
+            vga_console_info.video_type = XEN_VGATYPE_EFI_LFB;
+            vga_console_info.u.vesa_lfb.gbl_caps = 2; /* possibly non-VGA */
+            vga_console_info.u.vesa_lfb.width =
+                mode_info->HorizontalResolution;
+            vga_console_info.u.vesa_lfb.height = mode_info->VerticalResolution;
+            vga_console_info.u.vesa_lfb.bits_per_pixel = bpp;
+            vga_console_info.u.vesa_lfb.bytes_per_line =
+                (mode_info->PixelsPerScanLine * bpp + 7) >> 3;
+            vga_console_info.u.vesa_lfb.lfb_base = gop->Mode->FrameBufferBase;
+            vga_console_info.u.vesa_lfb.lfb_size =
+                (gop->Mode->FrameBufferSize + 0xffff) >> 16;
+        }
+    }
+}