#include <sys/types.h>
#include <sys/io.h>
#include <sys/stat.h>
#include <sys/vm86.h>
#include <sys/syscall.h>
#include <sys/mman.h>
#include <ctype.h>
#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <netinet/in.h>
#include "bioscall.h"
#ident "$Id: bioscall.c,v 1.1.1.1 2006/01/07 11:12:35 dgel Exp $"

#define DFLAG 0x0400
#define IFLAG 0x0200
#define TFLAG 0x0100
#define SFLAG 0x0080
#define ZFLAG 0x0040
#define AFLAG 0x0010
#define PFLAG 0x0004
#define CFLAG 0x0001

/* Dump some of the interesting parts of a register struct to stdout. */
void dump_regs(struct vm86_regs *regs)
{
	printf("ax = 0x%04lx\n", regs->eax & 0xffff);
	printf("bx = 0x%04lx\n", regs->ebx & 0xffff);
	printf("cx = 0x%04lx\n", regs->ecx & 0xffff);
	printf("dx = 0x%04lx\n", regs->edx & 0xffff);
	printf("cs = 0x%04x\n", regs->cs  & 0xffff);
	printf("ip = 0x%08lx\n", regs->eip & 0xffffffff);
	printf("ss = 0x%04x\n", regs->ss  & 0xffff);
	printf("sp = 0x%08lx\n", regs->esp & 0xffffffff);
	printf("%04x:%08lx = (%ld)\n",
	       regs->cs & 0xffff, regs->eip,
	       regs->cs * 16 + regs->eip);
}

/* Call vm86, but do I/O that gets trapped. We could skip vm86() altogether,
   but then I'm not trying to emulate an entire CPU here.  Luckily, none of
   the I/O instructions (or push/pop) affect the flags, so we can leave them
   alone and just deal with performing the I/O operation that caused a return
   to 32-bit mode. */
void do_vm86(struct vm86_struct *vm, char *memory, unsigned stop_eip) {
	int ret;
	unsigned start_cs, start_eip;
	unsigned char *ip = NULL;

	/* Save the starting instruction address. */
	start_cs = vm->regs.cs;
	start_eip = vm->regs.eip;

	/* We'll need to pass I/O through.  PCI devices have higher addresses
	   than we can get access to with ioperm(). */
	if(iopl(3) != 0) {
		return;
	}

	/* Do it. */
	ret = syscall(SYS_vm86old, vm);
	while((vm->regs.cs * 16 + vm->regs.eip) != (start_cs * 16 + stop_eip)) {
		ip = &memory[vm->regs.cs * 16 + vm->regs.eip];
#ifdef DEBUG2
		printf("Unexpected return:\n");
		dump_regs(&vm->regs);
		printf("Offending instructions: %02x %02x %02x %02x\n",
		       ip[0], ip[1], ip[2], ip[3]);
#endif
		switch(ip[0]) {
			case 0xe4: { /* in al, literal */
				vm->regs.eax &= 0xffffff00;
				vm->regs.eax |= inb(ip[1]);
				vm->regs.eip += 2;
				break;
			}
			case 0xe6: { /* out al, literal */
				outb(vm->regs.eax & 0xff, ip[1]);
				vm->regs.eip += 2;
				break;
			}
			case 0xec: { /* in al, dx */
				vm->regs.eax &= 0xffffff00;
				vm->regs.eax |= inb(vm->regs.edx & 0xffff);
				vm->regs.eip++;
				break;
			}
			case 0xed: { /* in ax, dx */
				vm->regs.eax &= 0xffff0000;
				vm->regs.eax |= inw(vm->regs.edx & 0xffff);
				vm->regs.eip++;
				break;
			}
			case 0xee: { /* out al, dx */
				outb(vm->regs.eax & 0xff,
				     vm->regs.edx & 0xffff);
				vm->regs.eip++;
				break;
			}
			case 0xef: { /* out ax, dx */
				outw(vm->regs.eax & 0xffff,
				     vm->regs.edx & 0xffff);
				vm->regs.eip++;
				break;
			}
			case 0x6c: { /* insb */
				unsigned char *result = (unsigned char*)
					&memory[vm->regs.es*16 + vm->regs.edi];
				*result = inb(vm->regs.edx & 0xffff);
				if(vm->regs.eflags & DFLAG) {
					vm->regs.edi -= 1;
				} else {
					vm->regs.edi += 1;
				}
				if(ip[-1] == 0xf3) { /* rep'ped */
					vm->regs.ecx--;
					vm->regs.eip--;
				} else {
					vm->regs.eip++;
				}
				break;
			}
			case 0x6d: { /* insw */
				u_int16_t *result = (u_int16_t*)
					&memory[vm->regs.es*16 + vm->regs.edi];
				*result = inw(vm->regs.edx & 0xffff);
				if(vm->regs.eflags & DFLAG) {
					vm->regs.edi -= 2;
				} else {
					vm->regs.edi += 2;
				}
				if(ip[-1] == 0xf3) { /* rep'ped */
					vm->regs.ecx--;
					vm->regs.eip--;
				} else {
					vm->regs.eip++;
				}
				break;
			}
			case 0x6e: { /* outsb */
				unsigned char *result = (unsigned char*)
					&memory[vm->regs.es*16 + vm->regs.edi];
				outb(*result,
				     vm->regs.edx & 0xffff);
				if(vm->regs.eflags & DFLAG) {
					vm->regs.edi -= 1;
				} else {
					vm->regs.edi += 1;
				}
				if(ip[-1] == 0xf3) { /* rep'ped */
					vm->regs.ecx--;
					vm->regs.eip--;
				} else {
					vm->regs.eip++;
				}
				break;
			}
			case 0x6f: { /* outsw */
				u_int16_t *result = (u_int16_t*)
					&memory[vm->regs.es*16 + vm->regs.edi];
				outw(*result,
				     vm->regs.edx & 0xffff);
				if(vm->regs.eflags & DFLAG) {
					vm->regs.edi -= 2;
				} else {
					vm->regs.edi += 2;
				}
				if(ip[-1] == 0xf3) { /* rep'ped */
					vm->regs.ecx--;
					vm->regs.eip--;
				} else {
					vm->regs.eip++;
				}
				break;
			}
			case 0xfa: { /* cli */
				vm->regs.eflags &= ~(IFLAG);
				vm->regs.eip++;
				break;
			}
			case 0xfb: { /* sti */
				vm->regs.eflags |= ~(IFLAG);
				vm->regs.eip++;
				break;
			}
			case 0x9c: { /* pushf */
				vm->regs.esp -= 2;
				*(u_int16_t*) &memory[vm->regs.ss * 16 +
						      vm->regs.esp]
						    = vm->regs.eflags & 0xffff;
				vm->regs.eip++;
				break;
			}
			case 0x9d: { /* popf */
				vm->regs.esp += 2;
				vm->regs.eflags &= 0xffff0000;
				vm->regs.eflags |= 
				*(u_int16_t*) &memory[vm->regs.ss * 16 +
						      vm->regs.esp];
				vm->regs.eip++;
				break;
			}
			case 0xf0: { /* lock prefix */
				/* ignore it */
				vm->regs.eip++;
				break;
			}
			case 0x66: {
				/* 32-bit extension prefix.  Valid, even in
				   v86 mode.  Weird. */
				vm->regs.eip++;
				ip++;
				switch(ip[0]) {
					case 0xed: { /* in eax, dx */
						vm->regs.eax =
						inl(vm->regs.edx & 0xffff);
						vm->regs.eip++;
						break;
					}
					case 0xef: { /* out eax, dx */
						outl(vm->regs.eax,
						     vm->regs.edx & 0xffff);
						vm->regs.eip++;
						break;
					}
					default: {
						fprintf(stderr, "unhandled "
							"32-bit opcode\n");
						exit(1);
					}
				}
				break;
			}
			case 0x55: { /* push bp */
				vm->regs.esp -= 2;
				*(u_int16_t*) &memory[vm->regs.ss * 16 +
						      vm->regs.esp]
						    = vm->regs.ebp & 0xffff;
				vm->regs.eip++;
				break;
			}
			case 0x5d: { /* pop bp */
				vm->regs.ebp &= 0xffff0000;
				vm->regs.ebp |= *(u_int16_t*)
					&memory[vm->regs.ss*16 + vm->regs.esp];
				vm->regs.esp += 2;
				vm->regs.eip++;
				break;
			}
			case 0x59: { /* pop cx -- Banshee */
				vm->regs.ecx &= 0xffff0000;
				vm->regs.ecx |= *(u_int16_t*)
					&memory[vm->regs.ss*16 + vm->regs.esp];
				vm->regs.esp += 2;
				vm->regs.eip++;
			}
			case 0xc3: { /* ret near, just pop ip */
				vm->regs.eip &= 0xffff0000;
				vm->regs.eip |= *(u_int16_t*)
					&memory[vm->regs.ss*16 + vm->regs.esp];
				vm->regs.esp += 2;
				break;
			}
			case 0xcb: { /* ret far, pop both ip and cs */
				vm->regs.eip &= 0xffff0000;
				vm->regs.eip |= *(u_int16_t*)
					&memory[vm->regs.ss*16 + vm->regs.esp];
				vm->regs.esp += 2;
				vm->regs.cs = *(u_int16_t*)
					&memory[vm->regs.ss*16 + vm->regs.esp];
				vm->regs.esp += 2;
				break;
			}
			default: {
				fprintf(stderr, "Unexpected stop!\n");
				dump_regs(&vm->regs);
				printf("Offending instructions: %02x %02x %02x %02x\n",
				       ip[0], ip[1], ip[2], ip[3]);
				exit(1);
			}
		}
		ip = &memory[vm->regs.cs * 16 + vm->regs.eip];
#ifdef DEBUG
		printf("Resuming execution:\n");
		dump_regs(&vm->regs);
		printf("Offending instructions: %02x %02x %02x %02x\n",
		       ip[0], ip[1], ip[2], ip[3]);
#endif
		ret = syscall(SYS_vm86old, vm);
	}
#ifdef DEBUG
	printf("Reached stopping point, returning.\n");
#endif
	return;
}

/* Get a snapshot of the first megabyte of memory for use with vm86. */
unsigned char *vm86_ram_alloc()
{
	unsigned char *memory;
	int fd;

	/* Grab address 0 for this process.  mmap() 1 megabyte + 64k HMA */
	memory = mmap(0, 0x110000, PROT_READ | PROT_EXEC | PROT_WRITE,
		      MAP_PRIVATE | MAP_FIXED | MAP_ANON, -1, 0x00000);
	if(memory == MAP_FAILED) {
		perror("error mmap()ing memory for the BIOS");
		return MAP_FAILED;
	}

	/* Copy the low megabyte to our mmap()'ed buffer. */
	fd = open("/dev/mem", O_RDONLY);
	if(fd == -1) {
		perror("reading kernel memory");
		return MAP_FAILED;
	}
	// read(fd, memory, 0x110000);
	lseek(fd, 0, SEEK_SET);
	read(fd, &memory[0], 0x10000);
	lseek(fd, 0xa0000, SEEK_SET);
	read(fd, &memory[0xa0000], 0x50000);
	close(fd);

	return memory;
}

void vm86_ram_free(unsigned char *ram)
{
	munmap(ram, 0x110000);
}

void bioscall(unsigned char int_no, struct vm86_regs *regs, unsigned char *mem)
{
	unsigned char call[] = {0xcd, int_no, 0xcd, 0x09};
	struct vm86_struct vm;
	memset(&vm, 0, sizeof(vm));
	memcpy(&vm.regs, regs, sizeof(vm.regs));
	vm.regs.cs  = BIOSCALL_START_SEG;
	vm.regs.eip = BIOSCALL_START_OFS;
	vm.regs.ss  = BIOSCALL_START_SEG;
	vm.regs.esp = 0xfff0 - BIOSCALL_START_OFS;
	vm.regs.eflags = VM_MASK | IOPL_MASK;
	memcpy(&mem[BIOSCALL_START_SEG * 16 + BIOSCALL_START_OFS], call,
	       sizeof(call));
	do_vm86(&vm, mem, BIOSCALL_START_OFS + sizeof(call));
	memcpy(regs, &vm.regs, sizeof(vm.regs));
}
