x86: Add support for running option ROMs natively

On x86 machines we can use an emulator to run option ROMS as with other
architectures. But with some additional effort (mostly due to the 16-bit
nature of option ROMs) we can run them natively. Add support for this.

Signed-off-by: Simon Glass <sjg@chromium.org>

arch/x86/lib/Makefile

@@ -5,6 +5,9 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
+obj-y += bios.o
+obj-y += bios_asm.o
+obj-y += bios_interrupts.o
 obj-$(CONFIG_CMD_BOOTM) += bootm.o
 obj-y	+= cmd_boot.o
 obj-y	+= gcc.o

arch/x86/lib/bios.c

@@ -0,0 +1,347 @@
+/*
+ * From Coreboot file device/oprom/realmode/x86.c
+ *
+ * Copyright (C) 2007 Advanced Micro Devices, Inc.
+ * Copyright (C) 2009-2010 coresystems GmbH
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+#include <common.h>
+#include <bios_emul.h>
+#include <vbe.h>
+#include <asm/cache.h>
+#include <asm/processor.h>
+#include <asm/i8259.h>
+#include <asm/io.h>
+#include <asm/post.h>
+#include "bios.h"
+
+/* Interrupt handlers for each interrupt the ROM can call */
+static int (*int_handler[256])(void);
+
+/* to have a common register file for interrupt handlers */
+X86EMU_sysEnv _X86EMU_env;
+
+asmlinkage void (*realmode_call)(u32 addr, u32 eax, u32 ebx, u32 ecx, u32 edx,
+				 u32 esi, u32 edi);
+
+asmlinkage void (*realmode_interrupt)(u32 intno, u32 eax, u32 ebx, u32 ecx,
+				      u32 edx, u32 esi, u32 edi);
+
+static void setup_realmode_code(void)
+{
+	memcpy((void *)REALMODE_BASE, &asm_realmode_code,
+	       asm_realmode_code_size);
+
+	/* Ensure the global pointers are relocated properly. */
+	realmode_call = PTR_TO_REAL_MODE(asm_realmode_call);
+	realmode_interrupt = PTR_TO_REAL_MODE(__realmode_interrupt);
+
+	debug("Real mode stub @%x: %d bytes\n", REALMODE_BASE,
+	      asm_realmode_code_size);
+}
+
+static void setup_rombios(void)
+{
+	const char date[] = "06/11/99";
+	memcpy((void *)0xffff5, &date, 8);
+
+	const char ident[] = "PCI_ISA";
+	memcpy((void *)0xfffd9, &ident, 7);
+
+	/* system model: IBM-AT */
+	writeb(0xfc, 0xffffe);
+}
+
+static int int_exception_handler(void)
+{
+	/* compatibility shim */
+	struct eregs reg_info = {
+		.eax = M.x86.R_EAX,
+		.ecx = M.x86.R_ECX,
+		.edx = M.x86.R_EDX,
+		.ebx = M.x86.R_EBX,
+		.esp = M.x86.R_ESP,
+		.ebp = M.x86.R_EBP,
+		.esi = M.x86.R_ESI,
+		.edi = M.x86.R_EDI,
+		.vector = M.x86.intno,
+		.error_code = 0,
+		.eip = M.x86.R_EIP,
+		.cs = M.x86.R_CS,
+		.eflags = M.x86.R_EFLG
+	};
+	struct eregs *regs = &reg_info;
+
+	debug("Oops, exception %d while executing option rom\n", regs->vector);
+	cpu_hlt();
+
+	return 0;
+}
+
+static int int_unknown_handler(void)
+{
+	debug("Unsupported software interrupt #0x%x eax 0x%x\n",
+	      M.x86.intno, M.x86.R_EAX);
+
+	return -1;
+}
+
+/* setup interrupt handlers for mainboard */
+void bios_set_interrupt_handler(int intnum, int (*int_func)(void))
+{
+	int_handler[intnum] = int_func;
+}
+
+static void setup_interrupt_handlers(void)
+{
+	int i;
+
+	/*
+	 * The first 16 int_handler functions are not BIOS services,
+	 * but the CPU-generated exceptions ("hardware interrupts")
+	 */
+	for (i = 0; i < 0x10; i++)
+		int_handler[i] = &int_exception_handler;
+
+	/* Mark all other int_handler calls as unknown first */
+	for (i = 0x10; i < 0x100; i++) {
+		/* Skip if bios_set_interrupt_handler() isn't called first */
+		if (int_handler[i])
+			continue;
+
+		 /*
+		  * Now set the default functions that are actually needed
+		  * to initialize the option roms. The board may override
+		  * these with bios_set_interrupt_handler()
+		 */
+		switch (i) {
+		case 0x10:
+			int_handler[0x10] = &int10_handler;
+			break;
+		case 0x12:
+			int_handler[0x12] = &int12_handler;
+			break;
+		case 0x16:
+			int_handler[0x16] = &int16_handler;
+			break;
+		case 0x1a:
+			int_handler[0x1a] = &int1a_handler;
+			break;
+		default:
+			int_handler[i] = &int_unknown_handler;
+			break;
+		}
+	}
+}
+
+static void write_idt_stub(void *target, u8 intnum)
+{
+	unsigned char *codeptr;
+
+	codeptr = (unsigned char *)target;
+	memcpy(codeptr, &__idt_handler, __idt_handler_size);
+	codeptr[3] = intnum; /* modify int# in the code stub. */
+}
+
+static void setup_realmode_idt(void)
+{
+	struct realmode_idt *idts = NULL;
+	int i;
+
+	/*
+	 * Copy IDT stub code for each interrupt. This might seem wasteful
+	 * but it is really simple
+	 */
+	 for (i = 0; i < 256; i++) {
+		idts[i].cs = 0;
+		idts[i].offset = 0x1000 + (i * __idt_handler_size);
+		write_idt_stub((void *)((u32)idts[i].offset), i);
+	}
+
+	/*
+	 * Many option ROMs use the hard coded interrupt entry points in the
+	 * system bios. So install them at the known locations.
+	 */
+
+	/* int42 is the relocated int10 */
+	write_idt_stub((void *)0xff065, 0x42);
+	/* BIOS Int 11 Handler F000:F84D */
+	write_idt_stub((void *)0xff84d, 0x11);
+	/* BIOS Int 12 Handler F000:F841 */
+	write_idt_stub((void *)0xff841, 0x12);
+	/* BIOS Int 13 Handler F000:EC59 */
+	write_idt_stub((void *)0xfec59, 0x13);
+	/* BIOS Int 14 Handler F000:E739 */
+	write_idt_stub((void *)0xfe739, 0x14);
+	/* BIOS Int 15 Handler F000:F859 */
+	write_idt_stub((void *)0xff859, 0x15);
+	/* BIOS Int 16 Handler F000:E82E */
+	write_idt_stub((void *)0xfe82e, 0x16);
+	/* BIOS Int 17 Handler F000:EFD2 */
+	write_idt_stub((void *)0xfefd2, 0x17);
+	/* ROM BIOS Int 1A Handler F000:FE6E */
+	write_idt_stub((void *)0xffe6e, 0x1a);
+}
+
+static u8 vbe_get_mode_info(struct vbe_mode_info *mi)
+{
+	u16 buffer_seg;
+	u16 buffer_adr;
+	char *buffer;
+
+	debug("VBE: Getting information about VESA mode %04x\n",
+	      mi->video_mode);
+	buffer = PTR_TO_REAL_MODE(asm_realmode_buffer);
+	buffer_seg = (((unsigned long)buffer) >> 4) & 0xff00;
+	buffer_adr = ((unsigned long)buffer) & 0xffff;
+
+	realmode_interrupt(0x10, VESA_GET_MODE_INFO, 0x0000, mi->video_mode,
+			   0x0000, buffer_seg, buffer_adr);
+	memcpy(mi->mode_info_block, buffer, sizeof(struct vbe_mode_info));
+	mi->valid = true;
+
+	return 0;
+}
+
+static u8 vbe_set_mode(struct vbe_mode_info *mi)
+{
+	debug("VBE: Setting VESA mode %#04x\n", mi->video_mode);
+	/* request linear framebuffer mode */
+	mi->video_mode |= (1 << 14);
+	/* request clearing of framebuffer */
+	mi->video_mode &= ~(1 << 15);
+	realmode_interrupt(0x10, VESA_SET_MODE, mi->video_mode,
+			   0x0000, 0x0000, 0x0000, 0x0000);
+
+	return 0;
+}
+
+static void vbe_set_graphics(int vesa_mode, struct vbe_mode_info *mode_info)
+{
+	unsigned char *framebuffer;
+
+	mode_info->video_mode = (1 << 14) | vesa_mode;
+	vbe_get_mode_info(mode_info);
+
+	framebuffer = (unsigned char *)mode_info->vesa.phys_base_ptr;
+	debug("VBE: resolution:  %dx%d@%d\n",
+	      le16_to_cpu(mode_info->vesa.x_resolution),
+	      le16_to_cpu(mode_info->vesa.y_resolution),
+	      mode_info->vesa.bits_per_pixel);
+	debug("VBE: framebuffer: %p\n", framebuffer);
+	if (!framebuffer) {
+		debug("VBE: Mode does not support linear framebuffer\n");
+		return;
+	}
+
+	vbe_set_mode(mode_info);
+}
+
+void bios_run_on_x86(pci_dev_t pcidev, unsigned long addr, int vesa_mode,
+		     struct vbe_mode_info *mode_info)
+{
+	u32 num_dev;
+
+	num_dev = PCI_BUS(pcidev) << 8 | PCI_DEV(pcidev) << 3 |
+			PCI_FUNC(pcidev);
+
+	/* Needed to avoid exceptions in some ROMs */
+	interrupt_init();
+
+	/* Set up some legacy information in the F segment */
+	setup_rombios();
+
+	/* Set up C interrupt handlers */
+	setup_interrupt_handlers();
+
+	/* Set up real-mode IDT */
+	setup_realmode_idt();
+
+	/* Make sure the code is placed. */
+	setup_realmode_code();
+
+	disable_caches();
+	debug("Calling Option ROM at %lx, pci device %#x...", addr, num_dev);
+
+	/* Option ROM entry point is at OPROM start + 3 */
+	realmode_call(addr + 0x0003, num_dev, 0xffff, 0x0000, 0xffff, 0x0,
+		      0x0);
+	debug("done\n");
+
+	if (vesa_mode != -1)
+		vbe_set_graphics(vesa_mode, mode_info);
+}
+
+asmlinkage int interrupt_handler(u32 intnumber, u32 gsfs, u32 dses,
+				 u32 edi, u32 esi, u32 ebp, u32 esp,
+				 u32 ebx, u32 edx, u32 ecx, u32 eax,
+				 u32 cs_ip, u16 stackflags)
+{
+	u32 ip;
+	u32 cs;
+	u32 flags;
+	int ret = 0;
+
+	ip = cs_ip & 0xffff;
+	cs = cs_ip >> 16;
+	flags = stackflags;
+
+#ifdef CONFIG_REALMODE_DEBUG
+	debug("oprom: INT# 0x%x\n", intnumber);
+	debug("oprom: eax: %08x ebx: %08x ecx: %08x edx: %08x\n",
+	      eax, ebx, ecx, edx);
+	debug("oprom: ebp: %08x esp: %08x edi: %08x esi: %08x\n",
+	      ebp, esp, edi, esi);
+	debug("oprom:  ip: %04x      cs: %04x   flags: %08x\n",
+	      ip, cs, flags);
+	debug("oprom: stackflags = %04x\n", stackflags);
+#endif
+
+	/*
+	 * Fetch arguments from the stack and put them to a place
+	 * suitable for the interrupt handlers
+	 */
+	M.x86.R_EAX = eax;
+	M.x86.R_ECX = ecx;
+	M.x86.R_EDX = edx;
+	M.x86.R_EBX = ebx;
+	M.x86.R_ESP = esp;
+	M.x86.R_EBP = ebp;
+	M.x86.R_ESI = esi;
+	M.x86.R_EDI = edi;
+	M.x86.intno = intnumber;
+	M.x86.R_EIP = ip;
+	M.x86.R_CS = cs;
+	M.x86.R_EFLG = flags;
+
+	/* Call the interrupt handler for this interrupt number */
+	ret = int_handler[intnumber]();
+
+	/*
+	 * This code is quite strange...
+	 *
+	 * Put registers back on the stack. The assembler code will pop them
+	 * later. We force (volatile!) changing the values of the parameters
+	 * of this function. We know that they stay alive on the stack after
+	 * we leave this function.
+	 */
+	*(volatile u32 *)&eax = M.x86.R_EAX;
+	*(volatile u32 *)&ecx = M.x86.R_ECX;
+	*(volatile u32 *)&edx = M.x86.R_EDX;
+	*(volatile u32 *)&ebx = M.x86.R_EBX;
+	*(volatile u32 *)&esi = M.x86.R_ESI;
+	*(volatile u32 *)&edi = M.x86.R_EDI;
+	flags = M.x86.R_EFLG;
+
+	/* Pass success or error back to our caller via the CARRY flag */
+	if (ret) {
+		flags &= ~1; /* no error: clear carry */
+	} else {
+		debug("int%02x call returned error\n", intnumber);
+		flags |= 1;  /* error: set carry */
+	}
+	*(volatile u16 *)&stackflags = flags;
+
+	return ret;
+}

arch/x86/lib/bios.h

@@ -0,0 +1,98 @@
+/*
+ * From Coreboot file device/oprom/realmode/x86.h
+ *
+ * Copyright (C) 2007 Advanced Micro Devices, Inc.
+ * Copyright (C) 2009-2010 coresystems GmbH
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+
+#ifndef _X86_LIB_BIOS_H
+#define _X86_LIB_BIOS_H
+
+#define REALMODE_BASE		0x600
+
+#ifdef __ASSEMBLY__
+
+#define PTR_TO_REAL_MODE(x)	(x - asm_realmode_code + REALMODE_BASE)
+
+#else
+
+/* Convert a symbol address to our real mode area */
+#define PTR_TO_REAL_MODE(sym)\
+	(void *)(REALMODE_BASE + ((char *)&(sym) - (char *)&asm_realmode_code))
+
+/*
+ * The following symbols cannot be used directly. They need to be fixed up
+ * to point to the correct address location after the code has been copied
+ * to REALMODE_BASE. Absolute symbols are not used because those symbols are
+ * relocated by U-Boot.
+ */
+extern unsigned char asm_realmode_call, __realmode_interrupt;
+extern unsigned char asm_realmode_buffer;
+
+#define DOWNTO8(A) \
+	union { \
+		struct { \
+			union { \
+				struct { \
+					uint8_t A##l; \
+					uint8_t A##h; \
+				} __packed; \
+				uint16_t A##x; \
+			} __packed; \
+			uint16_t h##A##x; \
+		} __packed; \
+		uint32_t e##A##x; \
+	} __packed;
+
+#define DOWNTO16(A) \
+	union { \
+		struct { \
+			uint16_t A; \
+			uint16_t h##A; \
+		} __packed; \
+		uint32_t e##A; \
+	} __packed;
+
+struct eregs {
+	DOWNTO8(a);
+	DOWNTO8(c);
+	DOWNTO8(d);
+	DOWNTO8(b);
+	DOWNTO16(sp);
+	DOWNTO16(bp);
+	DOWNTO16(si);
+	DOWNTO16(di);
+	uint32_t vector;
+	uint32_t error_code;
+	uint32_t eip;
+	uint32_t cs;
+	uint32_t eflags;
+};
+
+struct realmode_idt {
+	u16 offset, cs;
+};
+
+void x86_exception(struct eregs *info);
+
+/* From x86_asm.S */
+extern unsigned char __idt_handler;
+extern unsigned int __idt_handler_size;
+extern unsigned char asm_realmode_code;
+extern unsigned int asm_realmode_code_size;
+
+asmlinkage void (*realmode_call)(u32 addr, u32 eax, u32 ebx, u32 ecx, u32 edx,
+				 u32 esi, u32 edi);
+
+asmlinkage void (*realmode_interrupt)(u32 intno, u32 eax, u32 ebx, u32 ecx,
+				      u32 edx, u32 esi, u32 edi);
+
+int int10_handler(void);
+int int12_handler(void);
+int int16_handler(void);
+int int1a_handler(void);
+#endif /*__ASSEMBLY__ */
+
+#endif

arch/x86/lib/bios_asm.S

@@ -0,0 +1,281 @@
+/*
+ * From coreboot x86_asm.S, cleaned up substantially
+ *
+ * Copyright (C) 2009-2010 coresystems GmbH
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+
+#include <asm/processor.h>
+#include <asm/processor-flags.h>
+#include "bios.h"
+
+#define SEG(segment)	$segment * X86_GDT_ENTRY_SIZE
+
+/*
+ * This is the interrupt handler stub code. It gets copied to the IDT and
+ * to some fixed addresses in the F segment. Before the code can used,
+ * it gets patched up by the C function copying it: byte 3 (the $0 in
+ * movb $0, %al) is overwritten with the interrupt numbers.
+ */
+
+	.code16
+	.globl __idt_handler
+__idt_handler:
+	pushal
+	movb 	$0, %al /* This instruction gets modified */
+	ljmp 	$0, $__interrupt_handler_16bit
+	.globl __idt_handler_size
+__idt_handler_size:
+	.long  . - __idt_handler
+
+.macro setup_registers
+	/* initial register values */
+	movl	44(%ebp), %eax
+	movl	%eax, __registers +  0 /* eax */
+	movl	48(%ebp), %eax
+	movl	%eax, __registers +  4 /* ebx */
+	movl	52(%ebp), %eax
+	movl	%eax, __registers +  8 /* ecx */
+	movl	56(%ebp), %eax
+	movl	%eax, __registers + 12 /* edx */
+	movl	60(%ebp), %eax
+	movl	%eax, __registers + 16 /* esi */
+	movl	64(%ebp), %eax
+	movl	%eax, __registers + 20 /* edi */
+.endm
+
+.macro	enter_real_mode
+	/* Activate the right segment descriptor real mode. */
+	ljmp	SEG(X86_GDT_ENTRY_16BIT_CS), $PTR_TO_REAL_MODE(1f)
+1:
+.code16
+	/*
+	 * Load the segment registers with properly configured segment
+	 * descriptors. They will retain these configurations (limits,
+	 * writability, etc.) once protected mode is turned off.
+	 */
+	mov	SEG(X86_GDT_ENTRY_16BIT_DS), %ax
+	mov	%ax, %ds
+	mov	%ax, %es
+	mov	%ax, %fs
+	mov	%ax, %gs
+	mov	%ax, %ss
+
+	/* Turn off protection */
+	movl	%cr0, %eax
+	andl	$~X86_CR0_PE, %eax
+	movl	%eax, %cr0
+
+	/* Now really going into real mode */
+	ljmp	$0, $PTR_TO_REAL_MODE(1f)
+1:
+	/*
+	 * Set up a stack: Put the stack at the end of page zero. That way
+	 * we can easily share it between real and protected, since the
+	 * 16-bit ESP at segment 0 will work for any case.
+	 */
+	mov	$0x0, %ax
+	mov	%ax, %ss
+
+	/* Load 16 bit IDT */
+	xor	%ax, %ax
+	mov	%ax, %ds
+	lidt	__realmode_idt
+
+.endm
+
+.macro	prepare_for_irom
+	movl	$0x1000, %eax
+	movl	%eax, %esp
+
+	/* Initialise registers for option rom lcall */
+	movl	__registers +  0, %eax
+	movl	__registers +  4, %ebx
+	movl	__registers +  8, %ecx
+	movl	__registers + 12, %edx
+	movl	__registers + 16, %esi
+	movl	__registers + 20, %edi
+
+	/* Set all segments to 0x0000, ds to 0x0040 */
+	push	%ax
+	xor	%ax, %ax
+	mov	%ax, %es
+	mov	%ax, %fs
+	mov	%ax, %gs
+	mov	SEG(X86_GDT_ENTRY_16BIT_FLAT_DS), %ax
+	mov	%ax, %ds
+	pop	%ax
+
+.endm
+
+.macro	enter_protected_mode
+	/* Go back to protected mode */
+	movl	%cr0, %eax
+	orl	$X86_CR0_PE, %eax
+	movl	%eax, %cr0
+
+	/* Now that we are in protected mode jump to a 32 bit code segment */
+	data32	ljmp	SEG(X86_GDT_ENTRY_32BIT_CS), $PTR_TO_REAL_MODE(1f)
+1:
+	.code32
+	mov	SEG(X86_GDT_ENTRY_32BIT_DS), %ax
+	mov	%ax, %ds
+	mov	%ax, %es
+	mov	%ax, %gs
+	mov	%ax, %ss
+	mov	SEG(X86_GDT_ENTRY_32BIT_FS), %ax
+	mov	%ax, %fs
+
+	/* restore proper idt */
+	lidt	idt_ptr
+.endm
+
+/*
+ * In order to be independent of U-Boot's position in RAM we relocate a part
+ * of the code to the first megabyte of RAM, so the CPU can use it in
+ * real-mode. This code lives at asm_realmode_code.
+ */
+	.globl asm_realmode_code
+asm_realmode_code:
+
+/* Realmode IDT pointer structure. */
+__realmode_idt = PTR_TO_REAL_MODE(.)
+	.word 1023	/* 16 bit limit */
+	.long 0		/* 24 bit base */
+	.word 0
+
+/* Preserve old stack */
+__stack = PTR_TO_REAL_MODE(.)
+	.long 0
+
+/* Register store for realmode_call and realmode_interrupt */
+__registers = PTR_TO_REAL_MODE(.)
+	.long 0 /*  0 - EAX */
+	.long 0 /*  4 - EBX */
+	.long 0 /*  8 - ECX */
+	.long 0 /* 12 - EDX */
+	.long 0 /* 16 - ESI */
+	.long 0 /* 20 - EDI */
+
+/* 256 byte buffer, used by int10 */
+	.globl asm_realmode_buffer
+asm_realmode_buffer:
+	.skip 256
+
+	.code32
+	.globl asm_realmode_call
+asm_realmode_call:
+	/* save all registers to the stack */
+	pusha
+	pushf
+	movl	%esp, __stack
+	movl	%esp, %ebp
+
+	/*
+	 * This function is called with regparm=0 and we have to skip the
+	 * 36 bytes from pushf+pusha. Hence start at 40.
+	 * Set up our call instruction.
+	 */
+	movl	40(%ebp), %eax
+	mov	%ax, __lcall_instr + 1
+	andl	$0xffff0000, %eax
+	shrl	$4, %eax
+	mov	%ax, __lcall_instr + 3
+
+	wbinvd
+
+	setup_registers
+	enter_real_mode
+	prepare_for_irom
+
+__lcall_instr = PTR_TO_REAL_MODE(.)
+	.byte 0x9a
+	.word 0x0000, 0x0000
+
+	enter_protected_mode
+
+	/* restore stack pointer, eflags and register values and exit */
+	movl	__stack, %esp
+	popf
+	popa
+	ret
+
+	.globl __realmode_interrupt
+__realmode_interrupt:
+	/* save all registers to the stack and store the stack pointer */
+	pusha
+	pushf
+	movl	%esp, __stack
+	movl	%esp, %ebp
+
+	/*
+	 * This function is called with regparm=0 and we have to skip the
+	 * 36 bytes from pushf+pusha. Hence start at 40.
+	 * Prepare interrupt calling code.
+	 */
+	movl	40(%ebp), %eax
+	movb	%al, __intXX_instr + 1 /* intno */
+
+	setup_registers
+	enter_real_mode
+	prepare_for_irom
+
+__intXX_instr = PTR_TO_REAL_MODE(.)
+	.byte 0xcd, 0x00 /* This becomes intXX */
+
+	enter_protected_mode
+
+	/* restore stack pointer, eflags and register values and exit */
+	movl	__stack, %esp
+	popf
+	popa
+	ret
+
+/*
+ * This is the 16-bit interrupt entry point called by the IDT stub code.
+ *
+ * Before this code code is called, %eax is pushed to the stack, and the
+ * interrupt number is loaded into %al. On return this function cleans up
+ * for its caller.
+ */
+	.code16
+__interrupt_handler_16bit = PTR_TO_REAL_MODE(.)
+	push	%ds
+	push	%es
+	push	%fs
+	push	%gs
+
+	/* Clear DF to not break ABI assumptions */
+	cld
+
+	/*
+	 * Clean up the interrupt number. We could do this in the stub, but
+	 * it would cost two more bytes per stub entry.
+	 */
+	andl	$0xff, %eax
+	pushl	%eax		/* ... and make it the first parameter */
+
+	enter_protected_mode
+
+	/* Call the C interrupt handler */
+	movl	$interrupt_handler, %eax
+	call	*%eax
+
+	enter_real_mode
+
+	/*
+	 * Restore all registers, including those manipulated by the C
+	 * handler
+	 */
+	popl	%eax
+	pop	%gs
+	pop	%fs
+	pop	%es
+	pop	%ds
+	popal
+	iret
+
+	.globl asm_realmode_code_size
+asm_realmode_code_size:
+	.long  . - asm_realmode_code

arch/x86/lib/bios_interrupts.c

@@ -0,0 +1,217 @@
+/*
+ * From Coreboot
+ *
+ * Copyright (C) 2001 Ronald G. Minnich
+ * Copyright (C) 2005 Nick.Barker9@btinternet.com
+ * Copyright (C) 2007-2009 coresystems GmbH
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+
+#include <common.h>
+#include <asm/pci.h>
+#include "bios_emul.h"
+
+/* errors go in AH. Just set these up so that word assigns will work */
+enum {
+	PCIBIOS_SUCCESSFUL = 0x0000,
+	PCIBIOS_UNSUPPORTED = 0x8100,
+	PCIBIOS_BADVENDOR = 0x8300,
+	PCIBIOS_NODEV = 0x8600,
+	PCIBIOS_BADREG = 0x8700
+};
+
+int int10_handler(void)
+{
+	static u8 cursor_row, cursor_col;
+	int res = 0;
+
+	switch ((M.x86.R_EAX & 0xff00) >> 8) {
+	case 0x01: /* Set cursor shape */
+		res = 1;
+		break;
+	case 0x02: /* Set cursor position */
+		if (cursor_row != ((M.x86.R_EDX >> 8) & 0xff) ||
+		    cursor_col >= (M.x86.R_EDX & 0xff)) {
+			debug("\n");
+		}
+		cursor_row = (M.x86.R_EDX >> 8) & 0xff;
+		cursor_col = M.x86.R_EDX & 0xff;
+		res = 1;
+		break;
+	case 0x03: /* Get cursor position */
+		M.x86.R_EAX &= 0x00ff;
+		M.x86.R_ECX = 0x0607;
+		M.x86.R_EDX = (cursor_row << 8) | cursor_col;
+		res = 1;
+		break;
+	case 0x06: /* Scroll up */
+		debug("\n");
+		res = 1;
+		break;
+	case 0x08: /* Get Character and Mode at Cursor Position */
+		M.x86.R_EAX = 0x0f00 | 'A'; /* White on black 'A' */
+		res = 1;
+		break;
+	case 0x09: /* Write Character and attribute */
+	case 0x0e: /* Write Character */
+		debug("%c", M.x86.R_EAX & 0xff);
+		res = 1;
+		break;
+	case 0x0f: /* Get video mode */
+		M.x86.R_EAX = 0x5002; /*80 x 25 */
+		M.x86.R_EBX &= 0x00ff;
+		res = 1;
+		break;
+	default:
+		printf("Unknown INT10 function %04x\n", M.x86.R_EAX & 0xffff);
+		break;
+	}
+	return res;
+}
+
+int int12_handler(void)
+{
+	M.x86.R_EAX = 64 * 1024;
+	return 1;
+}
+
+int int16_handler(void)
+{
+	int res = 0;
+
+	switch ((M.x86.R_EAX & 0xff00) >> 8) {
+	case 0x00: /* Check for Keystroke */
+		M.x86.R_EAX = 0x6120; /* Space Bar, Space */
+		res = 1;
+		break;
+	case 0x01: /* Check for Keystroke */
+		M.x86.R_EFLG |= 1 << 6; /* Zero Flag set (no key available) */
+		res = 1;
+		break;
+	default:
+		printf("Unknown INT16 function %04x\n", M.x86.R_EAX & 0xffff);
+
+break;
+	}
+	return res;
+}
+
+#define PCI_CONFIG_SPACE_TYPE1	(1 << 0)
+#define PCI_SPECIAL_CYCLE_TYPE1	(1 << 4)
+
+int int1a_handler(void)
+{
+	unsigned short func = (unsigned short)M.x86.R_EAX;
+	int retval = 1;
+	unsigned short devid, vendorid, devfn;
+	/* Use short to get rid of gabage in upper half of 32-bit register */
+	short devindex;
+	unsigned char bus;
+	pci_dev_t dev;
+	u32 dword;
+	u16 word;
+	u8 byte, reg;
+
+	switch (func) {
+	case 0xb101: /* PCIBIOS Check */
+		M.x86.R_EDX = 0x20494350;	/* ' ICP' */
+		M.x86.R_EAX &= 0xffff0000; /* Clear AH / AL */
+		M.x86.R_EAX |= PCI_CONFIG_SPACE_TYPE1 |
+				PCI_SPECIAL_CYCLE_TYPE1;
+		/*
+		 * last bus in the system. Hard code to 255 for now.
+		 * dev_enumerate() does not seem to tell us (publically)
+		 */
+		M.x86.R_ECX = 0xff;
+		M.x86.R_EDI = 0x00000000;	/* protected mode entry */
+		retval = 1;
+		break;
+	case 0xb102: /* Find Device */
+		devid = M.x86.R_ECX;
+		vendorid = M.x86.R_EDX;
+		devindex = M.x86.R_ESI;
+		dev = pci_find_device(vendorid, devid, devindex);
+		if (dev != -1) {
+			unsigned short busdevfn;
+			M.x86.R_EAX &= 0xffff00ff; /* Clear AH */
+			M.x86.R_EAX |= PCIBIOS_SUCCESSFUL;
+			/*
+			 * busnum is an unsigned char;
+			 * devfn is an int, so we mask it off.
+			 */
+			busdevfn = (PCI_BUS(dev) << 8) | PCI_DEV(dev) << 3 |
+				PCI_FUNC(dev);
+			debug("0x%x: return 0x%x\n", func, busdevfn);
+			M.x86.R_EBX = busdevfn;
+			retval = 1;
+		} else {
+			M.x86.R_EAX &= 0xffff00ff; /* Clear AH */
+			M.x86.R_EAX |= PCIBIOS_NODEV;
+			retval = 0;
+		}
+		break;
+	case 0xb10a: /* Read Config Dword */
+	case 0xb109: /* Read Config Word */
+	case 0xb108: /* Read Config Byte */
+	case 0xb10d: /* Write Config Dword */
+	case 0xb10c: /* Write Config Word */
+	case 0xb10b: /* Write Config Byte */
+		devfn = M.x86.R_EBX & 0xff;
+		bus = M.x86.R_EBX >> 8;
+		reg = M.x86.R_EDI;
+		dev = PCI_BDF(bus, devfn >> 3, devfn & 7);
+		if (!dev) {
+			debug("0x%x: BAD DEVICE bus %d devfn 0x%x\n", func,
+			      bus, devfn);
+			/* Or are we supposed to return PCIBIOS_NODEV? */
+			M.x86.R_EAX &= 0xffff00ff; /* Clear AH */
+			M.x86.R_EAX |= PCIBIOS_BADREG;
+			retval = 0;
+			return retval;
+		}
+		switch (func) {
+		case 0xb108: /* Read Config Byte */
+			byte = pci_read_config8(dev, reg);
+			M.x86.R_ECX = byte;
+			break;
+		case 0xb109: /* Read Config Word */
+			word = pci_read_config16(dev, reg);
+			M.x86.R_ECX = word;
+			break;
+		case 0xb10a: /* Read Config Dword */
+			dword = pci_read_config32(dev, reg);
+			M.x86.R_ECX = dword;
+			break;
+		case 0xb10b: /* Write Config Byte */
+			byte = M.x86.R_ECX;
+			pci_write_config8(dev, reg, byte);
+			break;
+		case 0xb10c: /* Write Config Word */
+			word = M.x86.R_ECX;
+			pci_write_config16(dev, reg, word);
+			break;
+		case 0xb10d: /* Write Config Dword */
+			dword = M.x86.R_ECX;
+			pci_write_config32(dev, reg, dword);
+			break;
+		}
+
+#ifdef CONFIG_REALMODE_DEBUG
+		debug("0x%x: bus %d devfn 0x%x reg 0x%x val 0x%x\n", func,
+		      bus, devfn, reg, M.x86.R_ECX);
+#endif
+		M.x86.R_EAX &= 0xffff00ff; /* Clear AH */
+		M.x86.R_EAX |= PCIBIOS_SUCCESSFUL;
+		retval = 1;
+		break;
+	default:
+		printf("UNSUPPORTED PCIBIOS FUNCTION 0x%x\n", func);
+		M.x86.R_EAX &= 0xffff00ff; /* Clear AH */
+		M.x86.R_EAX |= PCIBIOS_UNSUPPORTED;
+		retval = 0;
+		break;
+	}
+
+	return retval;
+}