From 6c7a932d0b8baaaee16eca0ba061bfa6e0e57bfd Mon Sep 17 00:00:00 2001
From: Toomas Soome <tsoome@FreeBSD.org>
Date: Wed, 27 Jan 2021 00:47:56 +0200
Subject: [PATCH] loader: start kernel in text mode when there is no vbefb vt
 driver

If kernel is built without VT vbefb driver, make sure
we start kernel in text mode.
---
 stand/common/gfx_fb.h         |  1 +
 stand/common/load_elf.c       | 63 ++++++++++++++++++++++++++++++-----
 stand/i386/libi386/bootinfo.c | 10 ++++++
 stand/loader.mk               |  9 +++++
 4 files changed, 74 insertions(+), 9 deletions(-)

diff --git a/stand/common/gfx_fb.h b/stand/common/gfx_fb.h
index 1424b8223136..d046865604ea 100644
--- a/stand/common/gfx_fb.h
+++ b/stand/common/gfx_fb.h
@@ -204,6 +204,7 @@ typedef struct teken_gfx {
 	struct gen_fb	tg_fb;
 	teken_funcs_t	*tg_functions;
 	void		*tg_private;
+	bool		tg_kernel_supported;	/* Loaded kernel is supported */
 } teken_gfx_t;
 
 extern font_list_t fonts;
diff --git a/stand/common/load_elf.c b/stand/common/load_elf.c
index cb542718fe2f..62fdb560ecff 100644
--- a/stand/common/load_elf.c
+++ b/stand/common/load_elf.c
@@ -39,6 +39,7 @@ __FBSDID("$FreeBSD$");
 #include <stand.h>
 #define FREEBSD_ELF
 #include <sys/link_elf.h>
+#include <gfx_fb.h>
 
 #include "bootstrap.h"
 
@@ -84,12 +85,14 @@ typedef struct elf_file {
 
 static int __elfN(loadimage)(struct preloaded_file *mp, elf_file_t ef,
     uint64_t loadaddr);
-static int __elfN(lookup_symbol)(struct preloaded_file *mp, elf_file_t ef,
-    const char* name, Elf_Sym* sym);
+static int __elfN(lookup_symbol)(elf_file_t ef, const char* name,
+    Elf_Sym *sym, unsigned char type);
 static int __elfN(reloc_ptr)(struct preloaded_file *mp, elf_file_t ef,
     Elf_Addr p, void *val, size_t len);
 static int __elfN(parse_modmetadata)(struct preloaded_file *mp, elf_file_t ef,
     Elf_Addr p_start, Elf_Addr p_end);
+static bool __elfN(parse_vt_drv_set)(struct preloaded_file *mp, elf_file_t ef,
+    Elf_Addr p_start, Elf_Addr p_end);
 static symaddr_fn __elfN(symaddr);
 static char	*fake_modname(const char *name);
 
@@ -872,12 +875,24 @@ __elfN(loadimage)(struct preloaded_file *fp, elf_file_t ef, uint64_t off)
 	ef->buckets = ef->hashtab + 2;
 	ef->chains = ef->buckets + ef->nbuckets;
 
-	if (__elfN(lookup_symbol)(fp, ef, "__start_set_modmetadata_set",
-	    &sym) != 0)
+	gfx_state.tg_kernel_supported = false;
+	if (__elfN(lookup_symbol)(ef, "__start_set_vt_drv_set", &sym,
+	    STT_NOTYPE) == 0) {
+		p_start = sym.st_value + ef->off;
+		if (__elfN(lookup_symbol)(ef, "__stop_set_vt_drv_set", &sym,
+		    STT_NOTYPE) == 0) {
+			p_end = sym.st_value + ef->off;
+			gfx_state.tg_kernel_supported =
+			    __elfN(parse_vt_drv_set)(fp, ef, p_start, p_end);
+		}
+	}
+
+	if (__elfN(lookup_symbol)(ef, "__start_set_modmetadata_set", &sym,
+	    STT_NOTYPE) != 0)
 		return 0;
 	p_start = sym.st_value + ef->off;
-	if (__elfN(lookup_symbol)(fp, ef, "__stop_set_modmetadata_set",
-	    &sym) != 0)
+	if (__elfN(lookup_symbol)(ef, "__stop_set_modmetadata_set", &sym,
+	    STT_NOTYPE) != 0)
 		return ENOENT;
 	p_end = sym.st_value + ef->off;
 
@@ -1072,6 +1087,36 @@ __elfN(load_modmetadata)(struct preloaded_file *fp, uint64_t dest)
 	return (err);
 }
 
+/*
+ * Walk through vt_drv_set, each vt driver structure starts with
+ * static 16 chars for driver name. If we have "vbefb", return true.
+ */
+static bool
+__elfN(parse_vt_drv_set)(struct preloaded_file *fp, elf_file_t ef,
+    Elf_Addr p_start, Elf_Addr p_end)
+{
+	Elf_Addr v, p;
+	char vd_name[16];
+	int error;
+
+	p = p_start;
+	while (p < p_end) {
+		COPYOUT(p, &v, sizeof(v));
+
+		error = __elfN(reloc_ptr)(fp, ef, p, &v, sizeof(v));
+		if (error == EOPNOTSUPP)
+			v += ef->off;
+		else if (error != 0)
+			return (false);
+		COPYOUT(v, &vd_name, sizeof(vd_name));
+		if (strncmp(vd_name, "vbefb", sizeof(vd_name)) == 0)
+			return (true);
+		p += sizeof(Elf_Addr);
+	}
+
+	return (false);
+}
+
 int
 __elfN(parse_modmetadata)(struct preloaded_file *fp, elf_file_t ef,
     Elf_Addr p_start, Elf_Addr p_end)
@@ -1185,8 +1230,8 @@ elf_hash(const char *name)
 static const char __elfN(bad_symtable)[] = "elf" __XSTRING(__ELF_WORD_SIZE)
     "_lookup_symbol: corrupt symbol table\n";
 int
-__elfN(lookup_symbol)(struct preloaded_file *fp, elf_file_t ef,
-    const char* name, Elf_Sym *symp)
+__elfN(lookup_symbol)(elf_file_t ef, const char* name, Elf_Sym *symp,
+    unsigned char type)
 {
 	Elf_Hashelt symnum;
 	Elf_Sym sym;
@@ -1213,7 +1258,7 @@ __elfN(lookup_symbol)(struct preloaded_file *fp, elf_file_t ef,
 			free(strp);
 			if (sym.st_shndx != SHN_UNDEF ||
 			    (sym.st_value != 0 &&
-			    ELF_ST_TYPE(sym.st_info) == STT_FUNC)) {
+			    ELF_ST_TYPE(sym.st_info) == type)) {
 				*symp = sym;
 				return 0;
 			}
diff --git a/stand/i386/libi386/bootinfo.c b/stand/i386/libi386/bootinfo.c
index 71e07cfb9702..57f926b76589 100644
--- a/stand/i386/libi386/bootinfo.c
+++ b/stand/i386/libi386/bootinfo.c
@@ -41,6 +41,16 @@ __FBSDID("$FreeBSD$");
 void
 bi_load_vbe_data(struct preloaded_file *kfp)
 {
+	if (!gfx_state.tg_kernel_supported) {
+		/*
+		 * Loaded kernel does not have vt/vbe backend,
+		 * switch console to text mode.
+		 */
+		if (vbe_available())
+			bios_set_text_mode(VGA_TEXT_MODE);
+		return;
+	}
+
 	if (vbe_available()) {
 		file_addmetadata(kfp, MODINFOMD_VBE_FB,
 		    sizeof(gfx_state.tg_fb), &gfx_state.tg_fb);
diff --git a/stand/loader.mk b/stand/loader.mk
index cde7a31dca7e..3a38a9bc9e63 100644
--- a/stand/loader.mk
+++ b/stand/loader.mk
@@ -17,23 +17,32 @@ CFLAGS.pnglite.c+= -DHAVE_MEMCPY -I${SRCTOP}/sys/contrib/zlib
 .if ${MACHINE} == "i386" || ${MACHINE_CPUARCH} == "amd64"
 SRCS+=	load_elf32.c load_elf32_obj.c reloc_elf32.c
 SRCS+=	load_elf64.c load_elf64_obj.c reloc_elf64.c
+CFLAGS.load_elf32.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
+CFLAGS.load_elf64.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE_CPUARCH} == "aarch64"
 SRCS+=	load_elf64.c reloc_elf64.c
+CFLAGS.load_elf64.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE_CPUARCH} == "arm"
 SRCS+=	load_elf32.c reloc_elf32.c
+CFLAGS.load_elf32.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE_CPUARCH} == "powerpc"
 SRCS+=	load_elf32.c reloc_elf32.c
 SRCS+=	load_elf64.c reloc_elf64.c
 SRCS+=	metadata.c
+CFLAGS.load_elf32.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
+CFLAGS.load_elf64.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE_ARCH:Mmips64*} != ""
 SRCS+= load_elf64.c reloc_elf64.c
 SRCS+=	metadata.c
+CFLAGS.load_elf64.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE} == "mips"
 SRCS+=	load_elf32.c reloc_elf32.c
 SRCS+=	metadata.c
+CFLAGS.load_elf32.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .elif ${MACHINE_CPUARCH} == "riscv"
 SRCS+=	load_elf64.c reloc_elf64.c
 SRCS+=	metadata.c
+CFLAGS.load_elf64.c += -I$(SRCTOP)/sys/teken -I${SRCTOP}/contrib/pnglite
 .endif
 
 .if ${LOADER_DISK_SUPPORT:Uyes} == "yes"