#include <stdio.h>
#include <string.h>
#include <inttypes.h>
#include <unistd.h>
#include <sys/mman.h>

#include <linux/unistd.h>
#include <asm/vm86.h>

_syscall2(int, vm86, int, func, struct vm86plus_struct *, v86)

static inline void set_bit(uint8_t *a, unsigned int bit)
{
    a[bit / 8] |= (1 << (bit % 8));
}

static inline uint8_t *seg_to_linear(unsigned int seg, unsigned int reg)
{
    return (uint8_t *)((seg << 4) + (reg & 0xffff));
}

int main()
{
    uint8_t *vm86_mem;
    int ret, seg, arg, insn;
    struct vm86plus_struct ctx;
    struct vm86_regs *r;

    vm86_mem = mmap((void *)0x00000000, 0x110000, 
                    PROT_WRITE | PROT_READ | PROT_EXEC, 
                    MAP_FIXED | MAP_ANON | MAP_PRIVATE, -1, 0);
    if (vm86_mem == MAP_FAILED) {
        perror("mmap");
        return 1;
    }

    memset(&ctx, 0, sizeof(ctx));
    /* init basic registers */
    r = &ctx.regs;
    r->eip = 0x100;
    r->esp = 0xfffe;
    seg = 256;
    r->cs = seg;
    r->ss = seg;
    r->ds = seg;
    r->es = seg;
    r->fs = seg;
    r->gs = seg;
    r->eflags = VIF_MASK;

    /* put return code */
    set_bit((uint8_t *)&ctx.int_revectored, 3);
    *seg_to_linear(r->cs, r->eip) = 0xcc;	/* int3 */
    *seg_to_linear(r->cs, r->eip + 1) = 0xf4;	/* hlt */

do_vm86:
    ret = vm86(VM86_ENTER, &ctx);
    arg = VM86_ARG(ret);
    insn = *seg_to_linear(r->cs, r->eip);
    switch(VM86_TYPE(ret)) {
        case VM86_INTx:
	    printf("vm86: INT 0x%x\n", VM86_ARG(ret));
            break;
        case VM86_STI:
        case VM86_SIGNAL:
            /* a signal came, we just ignore that */
	    goto do_vm86;
            break;
        case VM86_TRAP:
	    if (arg == 3)
		printf("vm86: Trap 3 - All OK\n");
	    else
		printf("Unknown trap %#x\n", arg);
	    break;
        case VM86_UNKNOWN:
	    if (insn == 0xf4)
		printf("vm86: HLT, test failed\n");
	    else
		printf("vm86: unknown result, insn=%#x\n", insn);
        default:
            fprintf(stderr, "unhandled vm86 return code (0x%x)\n", ret);
    }
    return 0;
}