/* $Id: ldt.c,v 1.8 2008/11/01 21:14:59 kostik Exp kostik $ */

#include <sys/param.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <machine/segments.h>
#include <machine/sysarch.h>
#include <errno.h>
#include <inttypes.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

char stack[64 * 1024];

char a[1];

int
s2ds(int sel)
{

	return (LSEL(sel, SEL_UPL));
}

unsigned char
readbyte(int sel, int offset)
{
	unsigned char res;

	__asm__ volatile(
	    "\tpushl	%%es\n"
	    "\tmovl	%1,%%es\n"
	    "\tmovb	%%es:(%2),%0\n"
	    "\tpopl	%%es\n"
	    : "=r"(res) : "r"(s2ds(sel)), "r"(offset));

	return (res);
}

void
writebyte(int sel, int offset, unsigned char val)
{

	__asm__ volatile(
	    "\tpushl	%%es\n"
	    "\tmovl	%0,%%es\n"
	    "\tmovb	%2,%%es:(%1)\n"
	    "\tpopl	%%es\n"
	    : : "r"(s2ds(sel)), "r"(offset), "r"(val) : "memory");
}

int
alloc_sel(char *base, size_t len, int type, int p)
{
	int sel;
	union descriptor descs[1], descsk[1];
	uintptr_t pb;

	memset(descs, 0, sizeof(descs));
	if (len > PAGE_SIZE) {
		len = roundup(len, PAGE_SIZE);
		len /= PAGE_SIZE;
		descs[0].sd.sd_lolimit = len & 0xffff;
		descs[0].sd.sd_hilimit = (len >> 16) & 0xf;
		descs[0].sd.sd_gran = 1;
	} else {
		descs[0].sd.sd_lolimit = len;
		descs[0].sd.sd_hilimit = 0;
		descs[0].sd.sd_gran = 0;
	}
	pb = (uintptr_t)base;
	descs[0].sd.sd_lobase = pb & 0xffffff;
	descs[0].sd.sd_hibase = (pb >> 24) & 0xff;
	descs[0].sd.sd_type = type;
	descs[0].sd.sd_dpl = SEL_UPL;
	descs[0].sd.sd_p = p;
	descs[0].sd.sd_def32 = 1;

	if ((sel = i386_set_ldt(LDT_AUTO_ALLOC, descs, 1)) == -1)
		fprintf(stderr, "i386_set_ldt: %s\n", strerror(errno));
	else if (i386_get_ldt(sel, descsk, 1) == -1) {
		fprintf(stderr, "i386_get_ldt: %s\n", strerror(errno));
		sel = -1;
	} else if (memcmp(descs, descsk, sizeof(descs)) != 0) {
		fprintf(stderr, "descs != descsk\n");
		sel = -1;
	} else
		fprintf(stderr, "selector %d\n", sel);

	return (sel);
}

int
test1(int tnum, int sel)
{
	unsigned char ar;

	writebyte(sel, 0, '1');
	ar = readbyte(sel, 0);
	if (ar == '1')
		fprintf(stderr, "test %d.1 ok\n", tnum);
	else
		fprintf(stderr, "test%d.1 failed, ar %x\n", tnum, ar);
	writebyte(sel, 0, '2');
	ar = readbyte(sel, 0);
	if (ar == '2')
		fprintf(stderr, "test %d.2 ok\n", tnum);
	else
		fprintf(stderr, "test%d.2 failed, ar %x\n", tnum, ar);
	return (sel);
}

int
test2_func(void *arg)
{
	int *sel;

	sel = arg;
	test1(2, *sel);
	rfork(0);
	test1(3, *sel);
	return (0);
}

void
test2(int sel)
{
	pid_t r;
	int status;

	r = rfork_thread(RFPROC | RFMEM, stack + sizeof(stack),
	    test2_func, &sel);
	if (r == -1) {
		fprintf(stderr, "rfork(RFPROC): %s\n", strerror(errno));
		return;
	} else {
		waitpid(r, &status, 0);
		if (WIFSIGNALED(status)) {
			fprintf(stderr, "test2: child terminated by %s\n",
			    strsignal(WTERMSIG(status)));
		}
	}
}

int
main(int argc, char *argv[])
{
	int sel;

	sel = alloc_sel(a, 1, SDT_MEMRWA, 1);
	if (sel == -1)
		return (1);

	test1(1, sel);
	test2(sel);
	return (0);
}