Implement V86 monitor and VESA mode detection.

3 files changed, 331 insertions, 0 deletions

diff --git a/src/kernel/dev/v86.c b/src/kernel/dev/v86.c
new file mode 100644
index 0000000..9498eaf
--- /dev/null
+++ b/src/kernel/dev/v86.c
@@ -0,0 +1,208 @@
+#include <string.h>
+#include <paging.h>
+
+#include <dev/v86.h>
+
+#define EFLAGS_VM 0x20000
+#define V86_VALID_FLAGS 0xDFF
+
+//  ---- Ugly big static data
+
+STATIC_MUTEX(v86_mutex);
+
+v86_regs_t v86_regs;
+bool v86_if;
+
+pagedir_t *v86_pagedir = 0;
+void* v86_alloc_addr;
+
+bool v86_retval;
+thread_t *v86_caller_thread = 0;
+
+pagedir_t *v86_prev_pagedir = 0;
+
+//  ---- Setup code
+
+void v86_run_bios_int(void*);
+void v86_ex_handler(registers_t *regs);
+void v86_pf_handler(void*, registers_t *regs, void* addr);
+void v86_asm_enter_v86(v86_regs_t*);
+
+bool v86_begin_session() {
+	mutex_lock(&v86_mutex);
+
+	if (v86_pagedir == 0) {
+		v86_pagedir = create_pagedir(v86_pf_handler, 0);
+		if (v86_pagedir == 0) return false;
+
+		v86_prev_pagedir = get_current_pagedir();
+		switch_pagedir(v86_pagedir);
+
+		for (void* addr = (void*)V86_ALLOC_ADDR; addr < (void*)V86_STACK_TOP; addr += PAGE_SIZE) {
+			pd_map_page(addr, (uint32_t)addr / PAGE_SIZE, true);
+		}
+		for (void* addr = (void*)V86_BIOS_BEGIN; addr < (void*)V86_BIOS_END; addr += PAGE_SIZE) {
+			pd_map_page(addr, (uint32_t)addr / PAGE_SIZE, true);
+		}
+		pd_map_page(0, 0, true);
+	}
+
+	v86_alloc_addr = (void*)V86_ALLOC_ADDR;
+
+	memset(&v86_regs, 0, sizeof(v86_regs));
+
+	return true;
+}
+
+void v86_end_session() {
+	switch_pagedir(v86_prev_pagedir);
+	v86_prev_pagedir = 0;
+	v86_caller_thread = 0;
+
+	mutex_unlock(&v86_mutex);
+}
+
+void* v86_alloc(size_t size) {
+	void* addr = v86_alloc_addr;
+
+	v86_alloc_addr += size;
+
+	return addr;
+}
+
+bool v86_bios_int(uint8_t int_no) {
+	v86_caller_thread = current_thread;
+
+	uint32_t int_no_32 = int_no;
+
+	thread_t *v86t = new_thread(v86_run_bios_int, (void*)int_no_32);
+	v86t->user_ex_handler = v86_ex_handler;
+
+	int st = enter_critical(CL_NOSWITCH);
+	start_thread(v86t);
+	wait_on(current_thread);
+	exit_critical(st);
+
+	return v86_retval;
+}
+
+void v86_run_bios_int(void* x) {
+	switch_pagedir(v86_pagedir);
+
+	uint32_t int_no = (uint32_t)x;
+
+	uint16_t *ivt = (uint16_t*)0;
+
+	v86_regs.cs = ivt[2 * int_no + 1];
+	v86_regs.ip = ivt[2 * int_no];
+	v86_regs.ss = ((V86_STACK_TOP - 0x10000) >> 4);
+	v86_regs.sp = 0;
+
+	v86_if = true;
+
+	v86_asm_enter_v86(&v86_regs);
+}
+
+void v86_exit_thread(bool status) {
+	v86_retval = status;
+
+	resume_on(v86_caller_thread);
+	exit();
+}
+
+bool v86_gpf_handler(registers_t *regs) {
+	uint8_t* ip = (uint8_t*)V86_LIN_OF_SEG_OFF(regs->cs, regs->eip);
+	uint16_t *stack = (uint16_t*)V86_LIN_OF_SEG_OFF(regs->ss, (regs->esp & 0xFFFF));
+	uint32_t *stack32 = (uint32_t*)stack;
+	bool is_operand32 = false; // bool is_address32 = false;
+
+	while (true) {
+		switch (ip[0]) {
+			case 0x66:		// O32
+				is_operand32 = true;
+				ip++; regs->eip = (uint16_t)(regs->eip + 1);
+				break;
+			case 0x67:		// A32
+				// is_address32 = true;
+				ip++; regs->eip = (uint16_t)(regs->eip + 1);
+				break;
+			case 0x9C:		// PUSHF
+				if (is_operand32) {
+					regs->esp = ((regs->esp & 0xFFFF) - 4) & 0xFFFF;
+					stack32--;
+					*stack32 = regs->eflags & V86_VALID_FLAGS;
+					if (v86_if)
+						*stack32 |= EFLAGS_IF;
+					else
+						*stack32 &= ~EFLAGS_IF;
+				} else {
+					regs->esp = ((regs->esp & 0xFFFF) - 2) & 0xFFFF;
+					stack--;
+					*stack = regs->eflags;
+					if (v86_if)
+						*stack |= EFLAGS_IF;
+					else
+						*stack &= ~EFLAGS_IF;
+				}
+				regs->eip = (uint16_t)(regs->eip + 1);
+				return true;
+			case 0x9D:		// POPF
+				if (is_operand32) {
+					regs->eflags = EFLAGS_IF | EFLAGS_VM | (stack32[0] & V86_VALID_FLAGS);
+					v86_if = (stack32[0] & EFLAGS_IF) != 0;
+					regs->esp = ((regs->esp & 0xFFFF) + 4) & 0xFFFF;
+				} else {
+					regs->eflags = EFLAGS_IF | EFLAGS_VM | stack[0];
+					v86_if = (stack[0] & EFLAGS_IF) != 0;
+					regs->esp = ((regs->esp & 0xFFFF) + 2) & 0xFFFF;
+				}
+				regs->eip = (uint16_t)(regs->eip + 1);
+				return true;
+			case 0xCF:		// IRET
+				v86_regs.ax = (uint16_t)regs->eax;
+				v86_regs.bx = (uint16_t)regs->ebx;
+				v86_regs.cx = (uint16_t)regs->ecx;
+				v86_regs.dx = (uint16_t)regs->edx;
+				v86_regs.di = (uint16_t)regs->edi;
+				v86_regs.si = (uint16_t)regs->esi;
+
+				v86_exit_thread(true);
+			case 0xFA:		// CLI
+				v86_if = false;
+				regs->eip = (uint16_t)(regs->eip + 1);
+				return true;
+			case 0xFB:		// STI
+				v86_if = true;
+				regs->eip = (uint16_t)(regs->eip + 1);
+				return true;
+			default:
+				return false;
+		}
+	}
+}
+
+void v86_ex_handler(registers_t *regs) {
+	if (regs->int_no == EX_GENERAL_PROTECTION) {
+		if (!v86_gpf_handler(regs)) v86_exit_thread(false);
+	} else {
+		v86_exit_thread(false);
+	}
+}
+
+void v86_pf_handler(void* zero, registers_t *regs, void* addr) {
+	if (addr < (void*)V86_STACK_TOP) {
+		pd_map_page(addr, (uint32_t)addr / PAGE_SIZE, true);
+	} else {
+		dbg_printf("Unexpected V86 PF at 0x%p\n", addr);
+
+		if (current_thread->user_ex_handler == v86_ex_handler) {
+			// we are in V86 thread
+			v86_exit_thread(false);
+		} else {
+			PANIC("V86 memory access exception.");
+		}
+	}
+}
+
+
+/* vim: set ts=4 sw=4 tw=0 noet :*/
diff --git a/src/kernel/dev/v86asm.s b/src/kernel/dev/v86asm.s
new file mode 100644
index 0000000..da5bc38
--- /dev/null
+++ b/src/kernel/dev/v86asm.s
@@ -0,0 +1,39 @@
+
+[BITS 32]
+
+[GLOBAL v86_asm_enter_v86]
+v86_asm_enter_v86:
+	mov eax, [esp + 4]
+
+	xor ebx, ebx
+	mov bx, [eax + 20]		; get GS
+	push ebx
+	mov bx, [eax + 18]		; get FS
+	push ebx
+	mov bx, [eax + 14]		; get DS
+	push ebx
+	mov bx, [eax + 16]		; get ES
+	push ebx
+	mov bx, [eax + 22]		; get SS
+	push ebx
+	mov bx, [eax + 26]		; get SP
+	push ebx
+	pushf
+	pop ebx
+	or ebx, 20200h
+	push ebx
+	xor ebx, ebx
+	mov bx, [eax + 12]		; get CS
+	push ebx
+	mov bx, [eax + 24]		; get IP
+	push ebx
+
+	mov bx, [eax + 2]
+	mov cx, [eax + 4]
+	mov dx, [eax + 6]
+	mov di, [eax + 8]
+	mov si, [eax + 10]
+	mov ax, [eax]
+
+	iret
+
diff --git a/src/kernel/dev/vesa.c b/src/kernel/dev/vesa.c
new file mode 100644
index 0000000..6b23a34
--- /dev/null
+++ b/src/kernel/dev/vesa.c
@@ -0,0 +1,84 @@
+#include <string.h>
+
+#include <dev/vesa.h>
+#include <dev/v86.h>
+
+//  ---- VESA data structures
+
+typedef struct {
+	char vbe_signature[4];				// == "VESA"
+	uint16_t vbe_version;
+	v86_farptr_t oem_string_ptr;
+	uint8_t capabilities[4];
+	v86_farptr_t video_mode_ptr;
+	uint16_t total_memory;				// as # of 64KB blocks
+} __attribute__((packed)) vbe_info_block_t;
+
+typedef struct {
+	uint16_t attributes;
+	uint8_t winA, winB;
+	uint16_t granularity;
+	uint16_t winsize;
+	uint16_t segmentA, segmentB;
+	v86_farptr_t real_fct_ptr;
+	uint16_t pitch; // bytes per scanline
+
+	uint16_t Xres, Yres;
+	uint8_t Wchar, Ychar, planes, bpp, banks;
+	uint8_t memory_model, bank_size, image_pages;
+	uint8_t reserved0;
+
+	uint8_t red_mask, red_position;
+	uint8_t green_mask, green_position;
+	uint8_t blue_mask, blue_position;
+	uint8_t rsv_mask, rsv_position;
+	uint8_t directcolor_attributes;
+
+	uint32_t physbase;		// the offset of the framebuffer in physical memory!
+	uint32_t reserved1;
+	uint16_t reserved2;
+} __attribute__((packed)) vbe_mode_info_block_t;
+
+//  ---- VESA code
+
+void vesa_detect(fs_t *iofs) {
+	if (!v86_begin_session()) return;
+	
+	vbe_info_block_t *i = (vbe_info_block_t*)v86_alloc(512);
+	memset(i, 0, 512);
+	strncpy(i->vbe_signature, "VBE2", 4);
+
+	v86_regs.ax = 0x4F00;
+	v86_regs.es = V86_SEG_OF_LIN(i);
+	v86_regs.di = V86_OFF_OF_LIN(i);
+
+	if (v86_bios_int(0x10)) {
+		if (v86_regs.ax == 0x004F) {
+			dbg_printf("Detected VESA (sig %s, ver 0x%x, oem %s) with %d kb ram.\n",
+				i->vbe_signature, i->vbe_version, v86_lin_of_fp(i->oem_string_ptr), 64 * i->total_memory);
+
+			vbe_mode_info_block_t *mi = (vbe_mode_info_block_t*)v86_alloc(256);
+
+			uint16_t *modes = (uint16_t*)v86_lin_of_fp(i->video_mode_ptr);
+			for (int n = 0; modes[n] != 0xFFFF; n++) {
+				v86_regs.ax = 0x4F01;
+				v86_regs.cx = modes[n];
+				v86_regs.es = V86_SEG_OF_LIN(mi);
+				v86_regs.di = V86_OFF_OF_LIN(mi);
+				if (v86_bios_int(0x10) && v86_regs.ax == 0x004F) {
+					dbg_printf("Mode 0x%x : %dx%dx%d at 0x%p\n", modes[n], mi->Xres, mi->Yres, mi->bpp, mi->physbase);
+				} else {
+					dbg_printf("Mode 0x%x : could not get info\n", modes[n]);
+				}
+			}
+		} else {
+			dbg_printf("Error in BIOS int 0x10/0x4F00.\n");
+		}
+	} else {
+		dbg_printf("Could not call BIOS int 0x10.\n");
+	}
+
+	v86_end_session();
+}
+
+/* vim: set ts=4 sw=4 tw=0 noet :*/