[PATCH 18/18] x86: add multiboot2 protocol support for EFI platforms

[Daniel Kiper]

Signed-off-by: Daniel Kiper <address@hidden>
---
 xen/arch/x86/boot/head.S          |  174 +++++++++++++++++++++++++++++++++++--
 xen/arch/x86/efi/efi-boot.h       |   29 +++++++
 xen/arch/x86/setup.c              |   23 ++---
 xen/arch/x86/x86_64/asm-offsets.c |    2 +
 xen/common/efi/boot.c             |   11 +++
 5 files changed, 222 insertions(+), 17 deletions(-)

diff --git a/xen/arch/x86/boot/head.S b/xen/arch/x86/boot/head.S
index 7861057..89f5aa7 100644
--- a/xen/arch/x86/boot/head.S
+++ b/xen/arch/x86/boot/head.S
@@ -8,6 +8,7 @@
 #include <asm/page.h>
 #include <asm/msr.h>
 #include <asm/cpufeature.h>
+#include <asm/processor.h>
 
         .text
         .code32
@@ -57,6 +58,9 @@ ENTRY(start)
         .long   .Lmultiboot2_info_req_end - .Lmultiboot2_info_req
         .long   MULTIBOOT2_TAG_TYPE_BASIC_MEMINFO
         .long   MULTIBOOT2_TAG_TYPE_MMAP
+        .long   MULTIBOOT2_TAG_TYPE_EFI_BS
+        .long   MULTIBOOT2_TAG_TYPE_EFI64
+        .long   MULTIBOOT2_TAG_TYPE_EFI64_IH
 .Lmultiboot2_info_req_end:
 
         .align  MULTIBOOT2_TAG_ALIGN
@@ -80,6 +84,19 @@ ENTRY(start)
         .long   0 /* Number of the lines - no preference. */
         .long   0 /* Number of bits per pixel - no preference. */
 
+        /* Do not disable EFI boot services. */
+        .align  MULTIBOOT2_TAG_ALIGN
+        .short  MULTIBOOT2_HEADER_TAG_EFI_BS
+        .short  MULTIBOOT2_HEADER_TAG_OPTIONAL
+        .long   8 /* Tag size. */
+
+        /* EFI64 entry point. */
+        .align  MULTIBOOT2_TAG_ALIGN
+        .short  MULTIBOOT2_HEADER_TAG_ENTRY_ADDRESS_EFI64
+        .short  MULTIBOOT2_HEADER_TAG_OPTIONAL
+        .long   12 /* Tag size. */
+        .long   sym_phys(__efi64_start)
+
         /* Multiboot2 header end tag. */
         .align  MULTIBOOT2_TAG_ALIGN
         .short  MULTIBOOT2_HEADER_TAG_END
@@ -94,6 +111,17 @@ ENTRY(start)
 gdt_boot_descr:
         .word   6*8-1
         .long   sym_phys(trampoline_gdt)
+        .long   0 /* Needed for 64-bit lgdt */
+
+cs32_switch_addr:
+        .long   sym_phys(cs32_switch)
+        .long   BOOT_CS32
+
+efi_st:
+        .quad   0
+
+efi_ih:
+        .quad   0
 
 .Lbad_cpu_msg: .asciz "ERR: Not a 64-bit CPU!"
 .Lbad_ldr_msg: .asciz "ERR: Not a Multiboot bootloader!"
@@ -124,6 +152,133 @@ print_err:
 .Lhalt: hlt
         jmp     .Lhalt
 
+        .code64
+
+__efi64_start:
+        cld
+
+        /* Bootloaders may set multiboot[12].mem_lower to a nonzero value */
+        xor     %edx,%edx
+
+        /* Check for Multiboot2 bootloader */
+        cmp     $MULTIBOOT2_BOOTLOADER_MAGIC,%eax
+        je      efi_multiboot2_proto
+
+        jmp     not_multiboot
+
+efi_multiboot2_proto:
+        /* Skip Multiboot2 information fixed part */
+        lea     MB2_fixed_sizeof(%ebx),%ecx
+
+0:
+        /* Get mem_lower from Multiboot2 information */
+        cmpl    $MULTIBOOT2_TAG_TYPE_BASIC_MEMINFO,(%ecx)
+        jne     1f
+
+        mov     MB2_mem_lower(%ecx),%edx
+        jmp     4f
+
+1:
+        /* Get EFI SystemTable address from Multiboot2 information */
+        cmpl    $MULTIBOOT2_TAG_TYPE_EFI64,(%ecx)
+        jne     2f
+
+        lea     MB2_efi64_st(%ecx),%esi
+        lea     efi_st(%rip),%edi
+        movsq
+
+        /* Do not go into real mode on EFI platform */
+        movb    $1,skip_realmode(%rip)
+
+        jmp     4f
+
+2:
+        /* Get EFI ImageHandle address from Multiboot2 information */
+        cmpl    $MULTIBOOT2_TAG_TYPE_EFI64_IH,(%ecx)
+        jne     3f
+
+        lea     MB2_efi64_ih(%ecx),%esi
+        lea     efi_ih(%rip),%edi
+        movsq
+        jmp     4f
+
+3:
+        /* Is it the end of Multiboot2 information? */
+        cmpl    $MULTIBOOT2_TAG_TYPE_END,(%ecx)
+        je      run_bs
+
+4:
+        /* Go to next Multiboot2 information tag */
+        add     MB2_tag_size(%ecx),%ecx
+        add     $(MULTIBOOT2_TAG_ALIGN-1),%ecx
+        and     $~(MULTIBOOT2_TAG_ALIGN-1),%ecx
+        jmp     0b
+
+run_bs:
+        push    %rax
+        push    %rdx
+
+        /* Initialize BSS (no nasty surprises!) */
+        lea     __bss_start(%rip),%rdi
+        lea     _end(%rip),%rcx
+        sub     %rdi,%rcx
+        xor     %rax,%rax
+        rep     stosb
+
+        mov     efi_ih(%rip),%rdi   /* EFI ImageHandle */
+        mov     efi_st(%rip),%rsi   /* EFI SystemTable */
+        call    efi_multiboot2
+
+        pop     %rcx
+        pop     %rax
+
+        shl     $10-4,%rcx          /* Convert multiboot2.mem_lower to 
bytes/16 */
+
+        cli
+
+        /* Initialise GDT */
+        lgdt    gdt_boot_descr(%rip)
+
+        /* Reload code selector */
+        ljmpl   *cs32_switch_addr(%rip)
+
+        .code32
+
+cs32_switch:
+        /* Initialise basic data segments */
+        mov     $BOOT_DS,%edx
+        mov     %edx,%ds
+        mov     %edx,%es
+        mov     %edx,%fs
+        mov     %edx,%gs
+        mov     %edx,%ss
+
+        mov     $sym_phys(cpu0_stack)+1024,%esp
+
+        /* Disable paging */
+        mov     %cr0,%edx
+        and     $(~X86_CR0_PG),%edx
+        mov     %edx,%cr0
+
+        push    %eax
+        push    %ecx
+
+        /* Disable Long Mode */
+        mov     $MSR_EFER,%ecx
+        rdmsr
+        and     $(~EFER_LME),%eax
+        wrmsr
+
+        pop     %ecx
+        pop     %eax
+
+        /* Turn off PAE */
+        mov     %cr4,%edx
+        and     $(~X86_CR4_PAE),%edx
+        mov     %edx,%cr4
+
+        jmp     trampoline_setup
+
 __start:
         cld
         cli
@@ -151,10 +306,10 @@ __start:
 multiboot1_proto:
         /* Get mem_lower from Multiboot information */
         testb   $MBI_MEMLIMITS,(%ebx)
-        jz      trampoline_setup    /* not available? BDA value will be fine */
+        jz      bios_platform       /* not available? BDA value will be fine */
 
         mov     MB_mem_lower(%ebx),%edx
-        jmp     trampoline_setup
+        jmp     bios_platform
 
 multiboot2_proto:
         /* Skip Multiboot2 information fixed part */
@@ -166,12 +321,12 @@ multiboot2_proto:
         jne     1f
 
         mov     MB2_mem_lower(%ecx),%edx
-        jmp     trampoline_setup
+        jmp     bios_platform
 
 1:
         /* Is it the end of Multiboot2 information? */
         cmpl    $MULTIBOOT2_TAG_TYPE_END,(%ecx)
-        je      trampoline_setup
+        je      bios_platform
 
         /* Go to next Multiboot2 information tag */
         add     MB2_tag_size(%ecx),%ecx
@@ -179,7 +334,7 @@ multiboot2_proto:
         and     $~(MULTIBOOT2_TAG_ALIGN-1),%ecx
         jmp     0b
 
-trampoline_setup:
+bios_platform:
         /* Set up trampoline segment 64k below EBDA */
         movzwl  0x40e,%ecx          /* EBDA segment */
         cmp     $0xa000,%ecx        /* sanity check (high) */
@@ -195,12 +350,13 @@ trampoline_setup:
          * multiboot structure (if available) and use the smallest.
          */
         cmp     $0x100,%edx         /* is the multiboot value too small? */
-        jb      2f                  /* if so, do not use it */
+        jb      trampoline_setup    /* if so, do not use it */
         shl     $10-4,%edx
         cmp     %ecx,%edx           /* compare with BDA value */
         cmovb   %edx,%ecx           /* and use the smaller */
 
-2:      /* Reserve 64kb for the trampoline */
+trampoline_setup:
+        /* Reserve 64kb for the trampoline */
         sub     $0x1000,%ecx
 
         /* From arch/x86/smpboot.c: start_eip had better be page-aligned! */
@@ -215,6 +371,9 @@ trampoline_setup:
         call    reloc               /* %ecx contains trampoline address */
         mov     %eax,sym_phys(multiboot_ptr)
 
+        cmpb    $1,sym_phys(skip_realmode)
+        je      1f
+
         /* Initialize BSS (no nasty surprises!) */
         mov     $sym_phys(__bss_start),%edi
         mov     $sym_phys(_end),%ecx
@@ -222,6 +381,7 @@ trampoline_setup:
         xor     %eax,%eax
         rep     stosb
 
+1:
         /* Interrogate CPU extended features via CPUID. */
         mov     $0x80000000,%eax
         cpuid
diff --git a/xen/arch/x86/efi/efi-boot.h b/xen/arch/x86/efi/efi-boot.h
index 6e98bc8..f50c10a 100644
--- a/xen/arch/x86/efi/efi-boot.h
+++ b/xen/arch/x86/efi/efi-boot.h
@@ -223,6 +223,9 @@ static void *__init efi_arch_allocate_mmap_buffer(UINTN 
*map_size)
 
 static void __init efi_arch_pre_exit_boot(void)
 {
+    if ( !efi_loader )
+        return;
+
     if ( !trampoline_phys )
     {
         if ( !cfg.addr )
@@ -650,6 +653,32 @@ static bool_t __init 
efi_arch_use_config_file(EFI_SYSTEM_TABLE *SystemTable)
     return 1; /* x86 always uses a config file */
 }
 
+void __init efi_multiboot2(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE 
*SystemTable)
+{
+    EFI_GRAPHICS_OUTPUT_PROTOCOL *gop;
+    UINTN cols, gop_mode = ~0, rows;
+
+    efi_platform = 1;
+    efi_loader = 0;
+
+    efi_init(ImageHandle, SystemTable);
+
+    efi_console_set_mode();
+
+    if ( StdOut->QueryMode(StdOut, StdOut->Mode->Mode,
+                           &cols, &rows) == EFI_SUCCESS )
+        efi_arch_console_init(cols, rows);
+
+    gop = efi_get_gop();
+    gop_mode = efi_find_gop_mode(gop, 0, 0, 0);
+    efi_arch_edd();
+    efi_tables();
+    setup_efi_pci();
+    efi_variables();
+    efi_set_gop_mode(gop, gop_mode);
+    efi_exit_boot(ImageHandle, SystemTable);
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/arch/x86/setup.c b/xen/arch/x86/setup.c
index aebd010..8991b12 100644
--- a/xen/arch/x86/setup.c
+++ b/xen/arch/x86/setup.c
@@ -663,20 +663,23 @@ void __init noreturn __start_xen(unsigned long mbi_p)
     if ( ((unsigned long)cpu0_stack & (STACK_SIZE-1)) != 0 )
         panic("Misaligned CPU0 stack.");
 
-    if ( efi_loader )
+    if ( efi_platform )
     {
-        set_pdx_range(xen_phys_start >> PAGE_SHIFT,
-                      (xen_phys_start + BOOTSTRAP_MAP_BASE) >> PAGE_SHIFT);
+        if ( efi_loader )
+        {
+            set_pdx_range(xen_phys_start >> PAGE_SHIFT,
+                          (xen_phys_start + BOOTSTRAP_MAP_BASE) >> PAGE_SHIFT);
 
-        /* Clean up boot loader identity mappings. */
-        destroy_xen_mappings(xen_phys_start,
-                             xen_phys_start + BOOTSTRAP_MAP_BASE);
+            /* Clean up boot loader identity mappings. */
+            destroy_xen_mappings(xen_phys_start,
+                                 xen_phys_start + BOOTSTRAP_MAP_BASE);
 
-        /* Make boot page tables match non-EFI boot. */
-        l3_bootmap[l3_table_offset(BOOTSTRAP_MAP_BASE)] =
-            l3e_from_paddr(__pa(l2_bootmap), __PAGE_HYPERVISOR);
+            /* Make boot page tables match non-EFI boot. */
+            l3_bootmap[l3_table_offset(BOOTSTRAP_MAP_BASE)] =
+                l3e_from_paddr(__pa(l2_bootmap), __PAGE_HYPERVISOR);
+        }
 
-        memmap_type = loader;
+        memmap_type = "EFI";
     }
     else if ( e820_raw_nr != 0 )
     {
diff --git a/xen/arch/x86/x86_64/asm-offsets.c 
b/xen/arch/x86/x86_64/asm-offsets.c
index ca3712f..d27596b 100644
--- a/xen/arch/x86/x86_64/asm-offsets.c
+++ b/xen/arch/x86/x86_64/asm-offsets.c
@@ -172,4 +172,6 @@ void __dummy__(void)
     DEFINE(MB2_fixed_sizeof, sizeof(multiboot2_fixed_t));
     OFFSET(MB2_tag_size, multiboot2_tag_t, size);
     OFFSET(MB2_mem_lower, multiboot2_tag_basic_meminfo_t, mem_lower);
+    OFFSET(MB2_efi64_st, multiboot2_tag_efi64_t, pointer);
+    OFFSET(MB2_efi64_ih, multiboot2_tag_efi64_ih_t, pointer);
 }
diff --git a/xen/common/efi/boot.c b/xen/common/efi/boot.c
index f8be3dd..c5725ca 100644
--- a/xen/common/efi/boot.c
+++ b/xen/common/efi/boot.c
@@ -75,6 +75,17 @@ static size_t wstrlen(const CHAR16 * s);
 static int set_color(u32 mask, int bpp, u8 *pos, u8 *sz);
 static bool_t match_guid(const EFI_GUID *guid1, const EFI_GUID *guid2);
 
+static void efi_init(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable);
+static void efi_console_set_mode(void);
+static EFI_GRAPHICS_OUTPUT_PROTOCOL *efi_get_gop(void);
+static UINTN efi_find_gop_mode(EFI_GRAPHICS_OUTPUT_PROTOCOL *gop,
+                               UINTN cols, UINTN rows, UINTN depth);
+static void efi_tables(void);
+static void setup_efi_pci(void);
+static void efi_variables(void);
+static void efi_set_gop_mode(EFI_GRAPHICS_OUTPUT_PROTOCOL *gop, UINTN 
gop_mode);
+static void efi_exit_boot(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE 
*SystemTable);
+
 static const EFI_BOOT_SERVICES *__initdata efi_bs;
 static EFI_HANDLE __initdata efi_ih;
 
-- 
1.7.10.4