[PATCH v2 2/3] multiboot2: parse console= and vga= options when setting GOP mode

[Roger Pau Monne <roger.pau@xxxxxxxxxx>]

Only set the GOP mode if vga is selected in the console option,
otherwise just fetch the information from the current mode in order to
make it available to dom0.

Introduce support for passing the command line to the efi_multiboot2()
helper, and parse the console= and vga= options if present.

Add support for the 'gfx' and 'current' vga options, ignore the 'keep'
option, and print a warning message about other options not being
currently implemented.

Signed-off-by: Roger Pau Monné <roger.pau@xxxxxxxxxx>
---
Changes since v1:
 - Do not return the last occurrence of a command line.
 - Rearrange the code for assembly processing of the cmdline and use
   lea.
 - Merge patches handling console= and vga= together.
---
 xen/arch/x86/boot/head.S          | 13 ++++-
 xen/arch/x86/efi/efi-boot.h       | 80 ++++++++++++++++++++++++++++++-
 xen/arch/x86/x86_64/asm-offsets.c |  1 +
 3 files changed, 90 insertions(+), 4 deletions(-)

diff --git a/xen/arch/x86/boot/head.S b/xen/arch/x86/boot/head.S
index 0fb7dd3029..9d13eee50b 100644
--- a/xen/arch/x86/boot/head.S
+++ b/xen/arch/x86/boot/head.S
@@ -221,9 +221,10 @@ __efi64_mb2_start:
         jmp     x86_32_switch
 
 .Lefi_multiboot2_proto:
-        /* Zero EFI SystemTable and EFI ImageHandle addresses. */
+        /* Zero EFI SystemTable, EFI ImageHandle addresses and cmdline. */
         xor     %esi,%esi
         xor     %edi,%edi
+        xor     %edx,%edx
 
         /* Skip Multiboot2 information fixed part. */
         lea     (MB2_fixed_sizeof+MULTIBOOT2_TAG_ALIGN-1)(%rbx),%ecx
@@ -261,6 +262,13 @@ __efi64_mb2_start:
         cmove   MB2_efi64_ih(%rcx),%rdi
         je      .Lefi_mb2_next_tag
 
+        /* Get command line from Multiboot2 information. */
+        cmpl    $MULTIBOOT2_TAG_TYPE_CMDLINE,MB2_tag_type(%rcx)
+        jne     .Lno_cmdline
+        lea     MB2_tag_string(%rcx),%rdx
+        jmp     .Lefi_mb2_next_tag
+.Lno_cmdline:
+
         /* Is it the end of Multiboot2 information? */
         cmpl    $MULTIBOOT2_TAG_TYPE_END,MB2_tag_type(%rcx)
         je      .Lrun_bs
@@ -324,7 +332,8 @@ __efi64_mb2_start:
 
         /*
          * efi_multiboot2() is called according to System V AMD64 ABI:
-         *   - IN:  %rdi - EFI ImageHandle, %rsi - EFI SystemTable.
+         *   - IN:  %rdi - EFI ImageHandle, %rsi - EFI SystemTable,
+         *          %rdx - MB2 cmdline
          */
         call    efi_multiboot2
 
diff --git a/xen/arch/x86/efi/efi-boot.h b/xen/arch/x86/efi/efi-boot.h
index c94e53d139..f46c1f021e 100644
--- a/xen/arch/x86/efi/efi-boot.h
+++ b/xen/arch/x86/efi/efi-boot.h
@@ -786,7 +786,30 @@ static bool __init 
efi_arch_use_config_file(EFI_SYSTEM_TABLE *SystemTable)
 
 static void __init efi_arch_flush_dcache_area(const void *vaddr, UINTN size) { 
}
 
-void __init efi_multiboot2(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE 
*SystemTable)
+/* Return the next occurrence of opt in cmd. */
+static const char __init *get_option(const char *cmd, const char *opt)
+{
+    const char *s = cmd, *o = NULL;
+
+    if ( !cmd || !opt )
+        return NULL;
+
+    while ( (s = strstr(s, opt)) != NULL )
+    {
+        if ( s == cmd || *(s - 1) == ' ' )
+        {
+            o = s + strlen(opt);
+            break;
+        }
+
+        s += strlen(opt);
+    }
+
+    return o;
+}
+
+void __init efi_multiboot2(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE 
*SystemTable,
+                           const char *cmdline)
 {
     EFI_GRAPHICS_OUTPUT_PROTOCOL *gop;
     EFI_HANDLE gop_handle;
@@ -807,7 +830,60 @@ void __init efi_multiboot2(EFI_HANDLE ImageHandle, 
EFI_SYSTEM_TABLE *SystemTable
 
     if ( gop )
     {
-        gop_mode = efi_find_gop_mode(gop, 0, 0, 0);
+        const char *opt = NULL, *last = cmdline;
+        /* Default console selection is "com1,vga". */
+        bool vga = true;
+
+        /* For the console option the last occurrence is the enforced one. */
+        while ( (last = get_option(last, "console=")) != NULL )
+            opt = last;
+
+        if ( opt )
+        {
+            const char *s = strstr(opt, "vga");
+
+            if ( !s || s > strpbrk(opt, " ") )
+                vga = false;
+        }
+
+        if ( vga )
+        {
+            unsigned int width = 0, height = 0, depth = 0;
+            bool keep_current = false;
+
+            last = cmdline;
+            while ( (last = get_option(last, "vga=")) != NULL )
+            {
+                if ( !strncmp(last, "gfx-", 4) )
+                {
+                    width = simple_strtoul(last + 4, &last, 10);
+                    if ( *last == 'x' )
+                        height = simple_strtoul(last + 1, &last, 10);
+                    if ( *last == 'x' )
+                        depth = simple_strtoul(last + 1, &last, 10);
+                    /* Allow depth to be 0 or unset. */
+                    if ( !width || !height )
+                        width = height = depth = 0;
+                    keep_current = false;
+                }
+                else if ( !strncmp(last, "current", 7) )
+                    keep_current = true;
+                else if ( !strncmp(last, "keep", 4) )
+                {
+                    /* Ignore. */
+                }
+                else
+                {
+                    /* Fallback to defaults if unimplemented. */
+                    width = height = depth = 0;
+                    keep_current = false;
+                    PrintStr(L"Warning: Cannot use selected vga option.\r\n");
+                }
+            }
+
+            if ( !keep_current )
+                gop_mode = efi_find_gop_mode(gop, width, height, depth);
+        }
 
         efi_arch_edid(gop_handle);
     }
diff --git a/xen/arch/x86/x86_64/asm-offsets.c 
b/xen/arch/x86/x86_64/asm-offsets.c
index 287dac101a..fbd6c54188 100644
--- a/xen/arch/x86/x86_64/asm-offsets.c
+++ b/xen/arch/x86/x86_64/asm-offsets.c
@@ -175,6 +175,7 @@ void __dummy__(void)
     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);
+    OFFSET(MB2_tag_string, multiboot2_tag_string_t, string);
     BLANK();
 
     OFFSET(DOMAIN_vm_assist, struct domain, vm_assist);
-- 
2.40.0