#include <sys/types.h>
#include <sys/param.h>
#include <sys/proc.h>
#include <sys/module.h>
#include <sys/sysent.h>
#include <sys/kernel.h>
#include <sys/systm.h>
#include <sys/malloc.h>

/*
 * The function for implementing the syscall.
 */

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

extern uint16_t	x86_getfs(void);
extern void *x86_gettid(void);
extern void x86_setfs(uint16_t);
extern void x86_critical_enter(void);
extern void x86_critical_exit(void);
extern void x86_getldt(struct gdt *, uint16_t *);
extern void x86_setldt(struct gdt *, uint16_t);

static int
hello (struct thread *td, void *arg)
{
	int		fs;
	void		*tid;

	fs = x86_getfs();
	printf ("hello kernel\n");
	printf("FS: %x\n", fs);
	x86_setfs((17 << 3) | 4);
	tid = x86_gettid();
	x86_setfs(fs);
	printf("TID: %p\n", tid);
	return 0;
}

/*
 * The `sysent' for the new syscall
 */

static struct sysent hello_sysent = {
	0,			/* sy_narg */
	hello			/* sy_call */
};

/*
 * The offset in sysent where the syscall is allocated.
 */

static int offset = NO_SYSCALL;

/*
 * The function called at load/unload.
 */


struct gdt      gtable;
uint16_t        ltable;
uint16_t	fs_keep;

struct x86desc {
	uint16_t		x_lolimit;
	uint16_t		x_base0;
	uint8_t			x_base1;
	uint8_t			x_flags;
	uint8_t			x_hilimit;
	uint8_t			x_base2;
};

struct tid {
        void                    *tid_except_list;
        uint16_t                tid_oldfs;
};
struct tid my_tid = { (void *)0x12345678, 0xff };

uint16_t ctxsw_utow(uint16_t);
uint16_t ctxsw_wtou(void);

uint16_t
ctxsw_utow(fs)
        uint16_t                fs;
{
	my_tid.tid_oldfs = fs;
	return(fs_keep);
}

uint16_t
ctxsw_wtou(void)
{
	uint16_t		fs;

	fs = my_tid.tid_oldfs;
	my_tid.tid_oldfs = 0;

        return(fs);
}

static struct x86desc		*newldt;
static struct x86desc		*oldldt;

static void
x86_oldgdt(void)
{
	struct x86desc		*gdt;

	/* Grab location of existing GDT. */

	x86_critical_enter();
	x86_getldt(&gtable, &ltable);
	x86_critical_exit();

	/* Restore old LDT. */

	gdt = gtable.base;
	gdt += ltable / 8;

	x86_critical_enter();

	gdt->x_lolimit -= 8;
	gdt->x_base0 = (vm_offset_t)(oldldt) & 0xFFFF;
	gdt->x_base1 = ((vm_offset_t)(oldldt) >> 16) & 0xFF;
	gdt->x_base2 = ((vm_offset_t)(oldldt) >> 24) & 0xFF;
	x86_setldt(&gtable, ltable);

	x86_critical_exit();

	free(newldt, M_DEVBUF);

	return;
}

static void
x86_newgdt(void)
{
	struct x86desc		*l, *gdt;
	int			newlen;

	/* Grab location of existing GDT. */

	x86_critical_enter();
	x86_getldt(&gtable, &ltable);
	x86_critical_exit();

	/* Get pointer to the GDT table. */

	gdt = gtable.base;

	/* Get pointer to the LDT table */

	gdt += ltable / 8;
	oldldt = (struct x86desc *)(gdt->x_base2 << 24 |
	    gdt->x_base1 << 16 | gdt->x_base0);

	/* Allocate a new LDT, with one extra slot for us. */

	newlen = gdt->x_lolimit + sizeof(struct x86desc) + 1;
	newldt = malloc(newlen, M_DEVBUF, M_NOWAIT);
	bzero(newldt, newlen);

	/* Copy existing LDT info to new LDT. */

	bcopy(oldldt, newldt,  gdt->x_lolimit + 1);

	/* Set up new GDT entry. */

	l = newldt + (gdt->x_lolimit + 1) / 8;
	fs_keep = (gdt->x_lolimit + 1) / 8;
	l->x_lolimit = sizeof(my_tid);
	l->x_hilimit = 0xC0;
	l->x_base0 = (vm_offset_t)(&my_tid) & 0xFFFF;
	l->x_base1 = ((vm_offset_t)(&my_tid) >> 16) & 0xFF;
	l->x_base2 = ((vm_offset_t)(&my_tid) >> 24) & 0xFF;
	l->x_flags = 0x93;

	/* Install new LDT. */

	x86_critical_enter();

	gdt->x_lolimit += 8;
	gdt->x_base0 = (vm_offset_t)(newldt) & 0xFFFF;
	gdt->x_base1 = ((vm_offset_t)(newldt) >> 16) & 0xFF;
	gdt->x_base2 = ((vm_offset_t)(newldt) >> 24) & 0xFF;

	x86_setldt(&gtable, ltable);
	x86_critical_exit();

	/* Whew. */

	return;
}

static int
load (struct module *module, int cmd, void *arg)
{
	int error = 0;

	switch (cmd) {
	case MOD_LOAD :

printf("SETTING NEW LDT!!!!!\n");
		x86_newgdt();
printf("OMG STILL ALIVE!\n"); 
		printf ("syscall loaded at %d\n", offset);
		break;
	case MOD_UNLOAD :
		x86_oldgdt();
		printf ("syscall unloaded from %d\n", offset);
		break;
	default :
		error = EINVAL;
		break;
	}
	return error;
}

SYSCALL_MODULE(syscall, &offset, &hello_sysent, load, NULL);