From 6f80738b228c04e3ff3f2d14eea2161d2cf4f81c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Roger=20Pau=20Monn=C3=A9?= <royger@FreeBSD.org>
Date: Mon, 21 Nov 2022 12:40:08 +0100
Subject: [PATCH] xen: fetch dom0 video console information from Xen

It's possible for Xen to switch the video mode set by the boot loader,
so that the information passed in the kernel metadata is no longer
valid.  Fetch the video mode used by Xen using an hypercall and update
the medatada for the kernel to use the correct video mode.

Sponsored by: Citrix Systems R&D
---
 sys/contrib/xen/platform.h |  5 +++
 sys/x86/xen/pv.c           | 71 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 76 insertions(+)

diff --git a/sys/contrib/xen/platform.h b/sys/contrib/xen/platform.h
index 81001335094b..ccd6be741876 100644
--- a/sys/contrib/xen/platform.h
+++ b/sys/contrib/xen/platform.h
@@ -622,6 +622,10 @@ struct xenpf_symdata {
 typedef struct xenpf_symdata xenpf_symdata_t;
 DEFINE_XEN_GUEST_HANDLE(xenpf_symdata_t);
 
+#define XENPF_get_dom0_console 64
+typedef struct dom0_vga_console_info xenpf_dom0_console_t;
+DEFINE_XEN_GUEST_HANDLE(xenpf_dom0_console_t);
+
 /*
  * ` enum neg_errnoval
  * ` HYPERVISOR_platform_op(const struct xen_platform_op*);
@@ -652,6 +656,7 @@ struct xen_platform_op {
         xenpf_core_parking_t          core_parking;
         xenpf_resource_op_t           resource_op;
         xenpf_symdata_t               symdata;
+        xenpf_dom0_console_t          dom0_console;
         uint8_t                       pad[128];
     } u;
 };
diff --git a/sys/x86/xen/pv.c b/sys/x86/xen/pv.c
index 28794db98700..d721e9bb530e 100644
--- a/sys/x86/xen/pv.c
+++ b/sys/x86/xen/pv.c
@@ -336,6 +336,75 @@ xen_pvh_parse_symtab(void)
 }
 #endif
 
+static void
+fixup_console(caddr_t kmdp)
+{
+	struct xen_platform_op op = {
+		.cmd = XENPF_get_dom0_console,
+	};
+	xenpf_dom0_console_t *console = &op.u.dom0_console;
+	union {
+		struct efi_fb efi;
+		struct vbe_fb vbe;
+	} *fb = NULL;
+	int ret;
+
+	ret = HYPERVISOR_platform_op(&op);
+	if (ret != 0) {
+		xc_printf("Failed to get dom0 video console info\n");
+		return;
+	}
+
+	switch (console->video_type) {
+	case XEN_VGATYPE_VESA_LFB:
+		fb = (__typeof__ (fb))preload_search_info(kmdp,
+		    MODINFO_METADATA | MODINFOMD_VBE_FB);
+
+		if (fb == NULL) {
+			xc_printf("No VBE FB in kernel metadata\n");
+			return;
+		}
+
+		_Static_assert(offsetof(struct vbe_fb, fb_bpp) ==
+		    offsetof(struct efi_fb, fb_mask_reserved) +
+		    sizeof(fb->efi.fb_mask_reserved),
+		    "Bad structure overlay\n");
+		fb->vbe.fb_bpp = console->u.vesa_lfb.bits_per_pixel;
+		/* FALLTHROUGH */
+	case XEN_VGATYPE_EFI_LFB:
+		if (fb == NULL) {
+			fb = (__typeof__ (fb))preload_search_info(kmdp,
+			    MODINFO_METADATA | MODINFOMD_EFI_FB);
+			if (fb == NULL) {
+				xc_printf("No EFI FB in kernel metadata\n");
+				return;
+			}
+		}
+
+		fb->efi.fb_addr = console->u.vesa_lfb.lfb_base |
+		    ((uint64_t)console->u.vesa_lfb.ext_lfb_base << 32);
+		fb->efi.fb_size = console->u.vesa_lfb.lfb_size << 16;
+		fb->efi.fb_height = console->u.vesa_lfb.height;
+		fb->efi.fb_width = console->u.vesa_lfb.width;
+		fb->efi.fb_stride = (console->u.vesa_lfb.bytes_per_line << 3) /
+		    console->u.vesa_lfb.bits_per_pixel;
+#define FBMASK(c) \
+    ((~0u << console->u.vesa_lfb.c ## _pos) & \
+    (~0u >> (32 - console->u.vesa_lfb.c ## _pos - \
+    console->u.vesa_lfb.c ## _size)))
+		fb->efi.fb_mask_red = FBMASK(red);
+		fb->efi.fb_mask_green = FBMASK(green);
+		fb->efi.fb_mask_blue = FBMASK(blue);
+		fb->efi.fb_mask_reserved = FBMASK(rsvd);
+#undef FBMASK
+		break;
+
+	default:
+		xc_printf("Video console type unsupported\n");
+		return;
+	}
+}
+
 static caddr_t
 xen_pvh_parse_preload_data(uint64_t modulep)
 {
@@ -414,6 +483,8 @@ xen_pvh_parse_preload_data(uint64_t modulep)
 		    strlcpy(bootmethod, "UEFI", sizeof(bootmethod));
 		else
 		    strlcpy(bootmethod, "BIOS", sizeof(bootmethod));
+
+		fixup_console(kmdp);
 	} else {
 		/* Parse the extra boot information given by Xen */
 		if (start_info->cmdline_paddr != 0)