#include <stdio.h>
#include <sys/types.h>
#include <sys/errno.h>
#include <stdlib.h>
#include <stdarg.h>

#define __stdcall __attribute((__stdcall__))
#define __regparm __attribute((regparm(3)))

__stdcall static int foo(int a, int b)
{
	printf("A: %d B: %d\n", a, b);
	return(32);
}

__stdcall static int foo_3(int a, int b, int c)
{
	printf("FOO3\n");
	printf("A: %d B: %d C:%d\n", a, b, c);
	return(32);
}

__regparm static int foo_4(int a, int b, int c)
{
	printf("FOO4\n");
	printf("A: %d B: %d C:%d\n", a, b, c);
	return(32);
}

#define REGCALL2(arg1, arg2)    0, arg2, arg1
#define REGARGS2(decl1, decl2)  int dummy1, decl2, decl1

__regparm static int foo_5(REGARGS2(int a, int b), int c)
{
	printf("FOO5, fastcall\n");
	printf("A: %d B: %d C:%d\n", a, b, c);
	return(32);
}

static int foo2(int a, int b)
{
	printf("A: %d B: %d\n", a, b);
	return(32);
}

static int foo3(int a, int b, int c)
{
	printf("A: %d B: %d C: %d\n", a, b, c);
	return(32);
}

void doprintf(int esp)
{
	printf("ESP: 0x%x\n", esp);
	return;
}

typedef void (*funcptr)(void);

struct tid {
	void			*tid_except_list;
	uint32_t		tid_oldfs;
};

struct gdt {
	uint16_t		limit;
	void			*base;
} __attribute__((__packed__));


struct gdt	gtable;
uint16_t	ltable;
static uint16_t	fs_keep;
struct tid my_tids[4];

extern uint16_t x86_getfs(void);
extern void x86_setfs(uint16_t);

void
ctxsw_utow(void)
{
	struct tid		*t;

	t = &my_tids[0];
	t->tid_oldfs = x86_getfs();
printf("THUNK!! FS VALUE: %x\n", t->tid_oldfs);

	x86_setfs(t->tid_oldfs);

	/* We are now in Windows land. */

	return;
}


void
ctxsw_wtou(void)
{
	struct tid		*t;

	t = &my_tids[0];
	x86_setfs(t->tid_oldfs);

	/* We are now in UNIX land. */

	printf("UNTHUNK: %x\n", x86_getfs());
	return;
}

extern void     x86_fastcall_wrap(void);
extern void     x86_fastcall_wrap_call(void);
extern void     x86_fastcall_wrap_arg(void);
extern void     x86_fastcall_wrap_end(void);

int
windrv_wrap_fast(func, wrap, argcnt)
        funcptr                 func;
        funcptr                 *wrap;
	int8_t			argcnt;
{
        funcptr                 p;
        vm_offset_t             *calladdr;
	uint8_t			*argaddr;
        vm_offset_t             wrapstart, wrapend, wrapcall, wraparg;

        wrapstart = (vm_offset_t)&x86_fastcall_wrap;
        wrapend = (vm_offset_t)&x86_fastcall_wrap_end;
        wrapcall = (vm_offset_t)&x86_fastcall_wrap_call;
        wraparg = (vm_offset_t)&x86_fastcall_wrap_arg;

        /* Allocate a new wrapper instance. */

        p = malloc((wrapend - wrapstart) /*, M_DEVBUF, M_NOWAIT*/);
        if (p == NULL)
                return(ENOMEM);

        /* Copy over the code. */

        bcopy(x86_fastcall_wrap, p, (wrapend - wrapstart));

        /* Insert the function address into the new wrapper instance. */

	calladdr = (vm_offset_t *)((char *)p + ((wrapcall - wrapstart) + 1));
        *calladdr = (vm_offset_t)func;

	argcnt -= 2;
	if (argcnt < 1)
		argcnt = 0;

	argaddr = (u_int8_t *)((char *)p + ((wraparg - wrapstart) + 1));
	*argaddr = argcnt * sizeof(uint32_t);

        *wrap = p;

        return(0);
}

extern void     x86_stdcall_wrap(void);
extern void     x86_stdcall_wrap_call(void);
extern void     x86_stdcall_wrap_arg(void);
extern void     x86_stdcall_wrap_end(void);

int
windrv_wrap(func, wrap, argcnt)
        funcptr                 func;
        funcptr                 *wrap;
	uint8_t			argcnt;
{
        funcptr                 p;
        vm_offset_t             *calladdr;
	uint8_t			*argaddr;
        vm_offset_t             wrapstart, wrapend, wrapcall, wraparg;

        wrapstart = (vm_offset_t)&x86_stdcall_wrap;
        wrapend = (vm_offset_t)&x86_stdcall_wrap_end;
        wrapcall = (vm_offset_t)&x86_stdcall_wrap_call;
        wraparg = (vm_offset_t)&x86_stdcall_wrap_arg;

        /* Allocate a new wrapper instance. */

        p = malloc((wrapend - wrapstart) /*, M_DEVBUF, M_NOWAIT*/);
        if (p == NULL)
                return(ENOMEM);

        /* Copy over the code. */

        bcopy(x86_stdcall_wrap, p, (wrapend - wrapstart));

        /* Insert the function address into the new wrapper instance. */

	calladdr = (vm_offset_t *)((char *)p + ((wrapcall - wrapstart) + 1));
        *calladdr = (vm_offset_t)func;

	argaddr = (u_int8_t *)((char *)p + ((wraparg - wrapstart) + 1));
	*argaddr = argcnt * sizeof(uint32_t);

        *wrap = p;

        return(0);
}

typedef int (*tf)(int, int);
typedef __stdcall int (*stf)(int, int);
typedef __regparm int (*ftf)(int, int, int, int);

extern uint32_t x86_stdcall_call2(void *, uint32_t, uint32_t);
extern uint32_t x86_stdcall_call3(void *, uint32_t, uint32_t, uint32_t);
extern uint32_t x86_stdcall_call(void *, ...);
extern uint32_t x86_regparm_call(void *, ...);
extern void x86_getldt(struct gdt *, uint16_t *);
extern void x86_setldt(struct gdt *, uint16_t);

int main(void)
{
	stf		moo;
	ftf		moo2;

	fs_keep = x86_getfs();
	printf("STARTING FS:  %x\n", fs_keep);
	my_tids[0].tid_oldfs = fs_keep;
	foo2(2, 3);
	foo(111, 222);

	windrv_wrap(foo2, &moo, 2);
	moo(3, 4);

	windrv_wrap_fast(foo3, &moo2, 3);
	moo2(REGCALL2(3, 4), 33331112);

	/*x86_stdcall_call2(foo, 555, 666);*/
	x86_stdcall_call(foo_3, 3, 123, 456, 789);
	x86_regparm_call(foo_4, 999, 867, 5309);
	x86_fastcall_call(foo_5, 999, 867, 5309);
	printf("DONE!!\n");

	x86_getldt(&gtable, &ltable);
printf("SIZE: %d BASE: %p\n", gtable.limit + 1, gtable.base);
printf("LDT index: %d\n", ltable / 8);
	/*x86_setldt(&gtable, ltable);*/
}