xref: /netbsd/sys/arch/usermode/usermode/thunk.c (revision b950503f)

/* $NetBSD: thunk.c,v 1.92 2019/05/08 13:40:16 isaki Exp $ */

/*-
 * Copyright (c) 2011 Jared D. McNeill <jmcneill@invisible.ca>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>
#ifdef __NetBSD__
__RCSID("$NetBSD: thunk.c,v 1.92 2019/05/08 13:40:16 isaki Exp $");
#endif

#define _KMEMUSER
#define _X86_64_MACHTYPES_H_
#define _I386_MACHTYPES_H_

#include "../include/types.h"

#include <sys/mman.h>
#include <stdarg.h>
#include <sys/reboot.h>
#include <sys/poll.h>
#include <sys/sysctl.h>
#include <sys/socket.h>
#include <sys/audioio.h>
#include <sys/shm.h>
#include <sys/ioctl.h>

#include <machine/vmparam.h>

#include <net/if.h>
#include <net/if_dl.h>
#include <net/if_ether.h>
#include <net/if_tap.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <aio.h>
#include <assert.h>
#include <ctype.h>
#include <err.h>
#include <errno.h>
#include <fcntl.h>
#include <ifaddrs.h>
#include <sched.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <ucontext.h>
#include <unistd.h>
#include <stdbool.h>

#include "../include/thunk.h"

#ifdef __NetBSD__
#define SYS_syscallemu	511
#endif

#ifndef __arraycount
#define __arraycount(x)	(sizeof((x)) / sizeof((x)[0]))
#endif

#ifndef MAP_ANON
#define MAP_ANON MAP_ANONYMOUS
#endif

//#define RFB_DEBUG

static ssize_t safe_recv(int s, void *buf, int len);
static ssize_t safe_send(int s, const void *msg, int len);

extern int boothowto;

void
thunk_printf_debug(const char *fmt, ...)
{
	if (boothowto & AB_DEBUG) {
		va_list ap;

		va_start(ap, fmt);
		vfprintf(stderr, fmt, ap);
		va_end(ap);
	}
}

void
thunk_printf(const char *fmt, ...)
{
	va_list ap;

	va_start(ap, fmt);
	vfprintf(stderr, fmt, ap);
	va_end(ap);
	fflush(stderr);
}

int
thunk_syscallemu_init(void *ustart, void *uend)
{
	int error;

	errno = 0;
	error = syscall(SYS_syscallemu, (uintptr_t)ustart, (uintptr_t)uend);
	if (error == -1 && errno == EACCES) {
		/* syscallemu already active for this pid */
		error = 0;
	}

	return error;
}

static void
thunk_to_timeval(const struct thunk_timeval *ttv, struct timeval *tv)
{
	tv->tv_sec = ttv->tv_sec;
	tv->tv_usec = ttv->tv_usec;
}

static void
thunk_from_timeval(const struct timeval *tv, struct thunk_timeval *ttv)
{
	ttv->tv_sec = tv->tv_sec;
	ttv->tv_usec = tv->tv_usec;
}

static void
thunk_to_itimerval(const struct thunk_itimerval *tit, struct itimerval *it)
{
	thunk_to_timeval(&tit->it_interval, &it->it_interval);
	thunk_to_timeval(&tit->it_value, &it->it_value);
}

static void
thunk_from_itimerval(const struct itimerval *it, struct thunk_itimerval *tit)
{
	thunk_from_timeval(&it->it_interval, &tit->it_interval);
	thunk_from_timeval(&it->it_value, &tit->it_value);
}

static void
thunk_to_termios(const struct thunk_termios *tt, struct termios *t)
{
	int i;

	t->c_iflag = tt->c_iflag;
	t->c_oflag = tt->c_oflag;
	t->c_cflag = tt->c_cflag;
	t->c_lflag = tt->c_lflag;
	for (i = 0; i < __arraycount(t->c_cc); i++)
		t->c_cc[i] = tt->c_cc[i];
	t->c_ispeed = tt->c_ispeed;
	t->c_ospeed= tt->c_ospeed;
}

static void
thunk_from_termios(const struct termios *t, struct thunk_termios *tt)
{
	int i;

	tt->c_iflag = t->c_iflag;
	tt->c_oflag = t->c_oflag;
	tt->c_cflag = t->c_cflag;
	tt->c_lflag = t->c_lflag;
	for (i = 0; i < __arraycount(tt->c_cc); i++)
		tt->c_cc[i] = t->c_cc[i];
	tt->c_ispeed = t->c_ispeed;
	tt->c_ospeed= t->c_ospeed;
}

static int
thunk_to_native_prot(int prot)
{
	int nprot = PROT_NONE;

	if (prot & THUNK_PROT_READ)
		nprot |= PROT_READ;
	if (prot & THUNK_PROT_WRITE)
		nprot |= PROT_WRITE;
	if (prot & THUNK_PROT_EXEC)
		nprot |= PROT_EXEC;

	return nprot;
}

static int
thunk_to_native_mapflags(int flags)
{
	int nflags = 0;

	if (flags & THUNK_MAP_ANON)
		nflags |= MAP_ANON;
	if (flags & THUNK_MAP_FIXED)
		nflags |= MAP_FIXED;
	if (flags & THUNK_MAP_FILE)
		nflags |= MAP_FILE;
	if (flags & THUNK_MAP_SHARED)
		nflags |= MAP_SHARED;
	if (flags & THUNK_MAP_PRIVATE)
		nflags |= MAP_PRIVATE;

	return nflags;
}

static int
thunk_to_native_madviseflags(int flags)
{
	int nflags = 0;

	if (flags & THUNK_MADV_NORMAL)
		nflags |= MADV_NORMAL;
	if (flags & THUNK_MADV_RANDOM)
		nflags |= MADV_RANDOM;
	if (flags & THUNK_MADV_SEQUENTIAL)
		nflags |= MADV_SEQUENTIAL;
	if (flags & THUNK_MADV_WILLNEED)
		nflags |= MADV_WILLNEED;
	if (flags & THUNK_MADV_DONTNEED)
		nflags |= MADV_DONTNEED;
	if (flags & THUNK_MADV_FREE)
		nflags |= MADV_FREE;

	return nflags;
}

int
thunk_setitimer(int which, const struct thunk_itimerval *value,
    struct thunk_itimerval *ovalue)
{
	struct itimerval it, oit;
	int error;

	thunk_to_itimerval(value, &it);
	error = setitimer(which, &it, &oit);
	if (error)
		return error;
	if (ovalue)
		thunk_from_itimerval(&oit, ovalue);

	return 0;
}

int
thunk_gettimeofday(struct thunk_timeval *tp, void *tzp)
{
	struct timeval tv;
	int error;

	error = gettimeofday(&tv, tzp);
	if (error)
		return error;

	thunk_from_timeval(&tv, tp);

	return 0;
}

unsigned int
thunk_getcounter(void)
{
	struct timespec ts;
	int error;

	error = clock_gettime(CLOCK_MONOTONIC, &ts);
	if (error == -1) {
		warn("clock_gettime CLOCK_MONOTONIC");
		abort();
	}

	return (unsigned int)(ts.tv_nsec % 1000000000ULL);
}

long
thunk_clock_getres_monotonic(void)
{
	struct timespec res;
	int error;

	error = clock_getres(CLOCK_MONOTONIC, &res);
	if (error == -1)
		return -1;

	return (long)(res.tv_sec * 1000000000ULL + res.tv_nsec);
}

timer_t
thunk_timer_attach(void)
{
	timer_t timerid;
	int error;

	error = timer_create(CLOCK_MONOTONIC, NULL, &timerid);
	if (error == -1) {
		warn("timer_create CLOCK_MONOTONIC");
		abort();
	}

	return timerid;
}

int
thunk_timer_start(timer_t timerid, int freq)
{
	struct itimerspec tim;

	tim.it_interval.tv_sec = 0;
	tim.it_interval.tv_nsec = 1000000000 / freq;
	tim.it_value = tim.it_interval;

	return timer_settime(timerid, TIMER_RELTIME, &tim, NULL);
}

int
thunk_timer_getoverrun(timer_t timerid)
{
	return timer_getoverrun(timerid);
}

int
thunk_usleep(useconds_t microseconds)
{
	return usleep(microseconds);
}

void
thunk_kill(pid_t pid, int sig)
{
	kill(pid, sig);
}

void
thunk_exit(int status)
{
	return exit(status);
}

void
thunk_abort(void)
{
	abort();
}

int
thunk_geterrno(void)
{
	return errno;
}

void
thunk_seterrno(int nerrno)
{
	errno = nerrno;
}

int
thunk_getcontext(ucontext_t *ucp)
{
	return getcontext(ucp);
}

int
thunk_setcontext(const ucontext_t *ucp)
{
	return setcontext(ucp);
}

void
thunk_makecontext(ucontext_t *ucp, void (*func)(void),
    int nargs, void *arg1, void *arg2, void *arg3, void *arg4)
{
	switch (nargs) {
	case 0:
		makecontext(ucp, func, 0);
		break;
	case 1:
		makecontext(ucp, func, 1, arg1);
		break;
	case 2:
		makecontext(ucp, func, 2, arg1, arg2);
		break;
	case 3:
		makecontext(ucp, func, 3, arg1, arg2, arg3);
		break;
	case 4:
		makecontext(ucp, func, 4, arg1, arg2, arg3, arg4);
		break;
	default:
		warnx("%s: nargs (%d) too big\n", __func__, nargs);
		abort();
	}
}

int
thunk_swapcontext(ucontext_t *oucp, ucontext_t *ucp)
{
	return swapcontext(oucp, ucp);
}

int
thunk_tcgetattr(int fd, struct thunk_termios *tt)
{
	struct termios t;
	int error;

	error = tcgetattr(fd, &t);
	if (error == -1)
		return error;
	thunk_from_termios(&t, tt);
	return 0;
}

int
thunk_tcsetattr(int fd, int action, const struct thunk_termios *tt)
{
	struct termios t;

	thunk_to_termios(tt, &t);
	return tcsetattr(fd, action, &t);
}

int
thunk_set_stdin_sigio(int onoff)
{
	int flags;

	flags = fcntl(STDIN_FILENO, F_GETFL, 0);

	if (onoff)
		flags |= O_ASYNC;
	else
		flags &= ~O_ASYNC;

	return fcntl(STDIN_FILENO, F_SETFL, flags);
}

int
thunk_pollchar(void)
{
	struct pollfd fds[1];
	uint8_t c;

	fds[0].fd = STDIN_FILENO;
	fds[0].events = POLLIN;
	fds[0].revents = 0;

	if (poll(fds, __arraycount(fds), 0) > 0) {
		if (fds[0].revents & POLLIN) {
			if (read(STDIN_FILENO, &c, 1) != 1)
				return EOF;
			return c;
		}
	}

	return EOF;
}

int
thunk_getchar(void)
{
	return getchar();
}

void
thunk_putchar(int c)
{
	char wc = (char) c;
	write(1, &wc, 1);
}

int
thunk_execv(const char *path, char * const argv[])
{
	return execv(path, argv);
}

int
thunk_open(const char *path, int flags, mode_t mode)
{
	return open(path, flags, mode);
}

int
thunk_close(int fd)
{
	return close(fd);
}

int
thunk_fstat_getsize(int fd, off_t *size, ssize_t *blksize)
{
	struct stat st;
	int error;

	error = fstat(fd, &st);
	if (error)
		return -1;

	if (size)
		*size = st.st_size;
	if (blksize)
		*blksize = st.st_blksize;

	return 0;
}

ssize_t
thunk_pread(int d, void *buf, size_t nbytes, off_t offset)
{
	return pread(d, buf, nbytes, offset);
}

ssize_t
thunk_pwrite(int d, const void *buf, size_t nbytes, off_t offset)
{
	return pwrite(d, buf, nbytes, offset);
}

ssize_t
thunk_read(int d, void *buf, size_t nbytes)
{
	return read(d, buf, nbytes);
}

ssize_t
thunk_write(int d, const void *buf, size_t nbytes)
{
	return write(d, buf, nbytes);
}

int
thunk_fsync(int fd)
{
	return fsync(fd);
}

int
thunk_mkstemp(char *template)
{
	return mkstemp(template);
}

int
thunk_unlink(const char *path)
{
	return unlink(path);
}

pid_t
thunk_getpid(void)
{
	return getpid();
}

int
thunk_sigaction(int sig, const struct sigaction *act, struct sigaction *oact)
{
	return sigaction(sig, act, oact);
}

int
thunk_sigaltstack(const stack_t *ss, stack_t *oss)
{
	return sigaltstack(ss, oss);
}

void
thunk_signal(int sig, void (*func)(int))
{
	struct sigaction sa;

	sa.sa_flags = SA_RESTART | SA_ONSTACK;
	sa.sa_sigaction = (void (*)(int, siginfo_t *, void *))func;
	sigemptyset(&sa.sa_mask);
	sigaction(sig, &sa, NULL);
}

int
thunk_sigblock(int sig)
{
	sigset_t set;

	sigemptyset(&set);
	sigaddset(&set, sig);
	return sigprocmask(SIG_BLOCK, &set, NULL);
}

int
thunk_sigunblock(int sig)
{
	sigset_t set;

	sigemptyset(&set);
	sigaddset(&set, sig);
	return sigprocmask(SIG_UNBLOCK, &set, NULL);
}

int
thunk_sigemptyset(sigset_t *sa_mask)
{
	return sigemptyset(sa_mask);
}


int
thunk_sigfillset(sigset_t *sa_mask)
{
	return sigfillset(sa_mask);
}


void
thunk_sigaddset(sigset_t *sa_mask, int sig)
{
	int retval;
	retval = sigaddset(sa_mask, sig);
	if (retval == -1) {
		warn("bad signal added");
		abort();
	}
}

int
thunk_sigprocmask(int how, const sigset_t * set, sigset_t *oset)
{
	return sigprocmask(how, set, oset);
}

int
thunk_atexit(void (*function)(void))
{
	return atexit(function);
}

pid_t
thunk_fork(void)
{
	return fork();
}

int
thunk_ioctl(int fd, unsigned long request, void *opaque)
{
	return ioctl(fd, request, opaque);
}

int
thunk_aio_read(struct aiocb *aiocbp)
{
	return aio_read(aiocbp);
}

int
thunk_aio_write(struct aiocb *aiocbp)
{
	return aio_write(aiocbp);
}

int
thunk_aio_error(const struct aiocb *aiocbp)
{
	return aio_error(aiocbp);
}

int
thunk_aio_return(struct aiocb *aiocbp)
{
	return aio_return(aiocbp);
}

void *
thunk_malloc(size_t len)
{
	return malloc(len);
}

void
thunk_free(void *addr)
{
	free(addr);
}

void *
thunk_sbrk(intptr_t len)
{
	return sbrk(len);
}

void *
thunk_mmap(void *addr, size_t len, int prot, int flags, int fd, off_t offset)
{
	int nflags, nprot;
	void *a;

	nprot = thunk_to_native_prot(prot);
	nflags = thunk_to_native_mapflags(flags);

	a = mmap(addr, len, nprot, nflags, fd, offset);
	if (a == MAP_FAILED)
		warn("mmap");
	return a;
}

int
thunk_munmap(void *addr, size_t len)
{
	return munmap(addr, len);
}

int
thunk_mprotect(void *addr, size_t len, int prot)
{
	int nprot;

	nprot = thunk_to_native_prot(prot);

	return mprotect(addr, len, nprot);
}

int
thunk_madvise(void *addr, size_t len, int behav)
{
	int nbehav;

	nbehav = thunk_to_native_madviseflags(behav);

	return madvise(addr, len, nbehav);
}

int
thunk_posix_memalign(void **ptr, size_t alignment, size_t size)
{
	return posix_memalign(ptr, alignment, size);
}

char *
thunk_getenv(const char *name)
{
	return getenv(name);
}

vaddr_t
thunk_get_vm_min_address(void)
{
	return VM_MIN_ADDRESS;
}

int
thunk_idle(void)
{
	sigset_t sigmask;

	sigemptyset(&sigmask);

	return sigsuspend(&sigmask);
}

int
thunk_getcpuinfo(char *cp, size_t *len)
{
	ssize_t rlen;
	int fd;

	fd = open("/proc/cpuinfo", O_RDONLY);
	if (fd == -1)
		goto out;
	rlen = read(fd, cp, *len);
	close(fd);

	if (rlen == -1)
		goto out;

	cp[rlen ? rlen - 1 : 0] = '\0';
	*len = rlen;
	return 0;
out:
	*len = 0;
	return -1;
}

int
thunk_getmachine(char *machine, size_t machinelen,
    char *machine_arch, size_t machine_archlen)
{
	size_t len;

	memset(machine, 0, machinelen);
	len = machinelen - 1;
	if (sysctlbyname("hw.machine", machine, &len, NULL, 0) == -1) {
		warn("sysctlbyname hw.machine failed");
		abort();
	}

	memset(machine_arch, 0, machine_archlen);
	len = machine_archlen - 1;
	if (sysctlbyname("hw.machine_arch", machine_arch, &len, NULL, 0) == -1) {
		warn("sysctlbyname hw.machine_arch failed");
		abort();
	}

	return 0;
}

int
thunk_setown(int fd)
{
	return fcntl(fd, F_SETOWN, getpid());
}

int
thunk_open_tap(const char *device)
{
	int fd, error, enable;

	/* open tap device */
	fd = open(device, O_RDWR);
	if (fd == -1)
		return -1;

	/* set async mode */
	enable = 1;
	error = ioctl(fd, FIOASYNC, &enable);
	if (error == -1)
		return -1;

	return fd;
}

int
thunk_pollin_tap(int fd, int timeout)
{
#if 0
	struct pollfd fds[1];

	fds[0].fd = fd;
	fds[0].events = POLLIN|POLLRDNORM;
	fds[0].revents = 0;

	return poll(fds, __arraycount(fds), timeout);
#else
	int error, len;

	error = ioctl(fd, FIONREAD, &len);
	if (error == -1)
		return 0;

	return len;
#endif
}

int
thunk_pollout_tap(int fd, int timeout)
{
	struct pollfd fds[1];

	fds[0].fd = fd;
	fds[0].events = POLLOUT|POLLWRNORM;
	fds[0].revents = 0;

	return poll(fds, __arraycount(fds), timeout);
}


/* simply make sure its present... yeah its silly */
int
thunk_assert_presence(vaddr_t from, size_t size)
{
	vaddr_t va;
	int t = 0;

	for (va = from; va < from + (vaddr_t) size; va += PAGE_SIZE) {
		t += *(int *) va;
	}
	return t;
}


int
thunk_audio_open(const char *path)
{
	return open(path, O_RDWR);
}

int
thunk_audio_close(int fd)
{
	return close(fd);
}

int
thunk_audio_config(int fd, const thunk_audio_config_t *pconf,
    const thunk_audio_config_t *rconf)
{
	struct audio_info info;
	int error;

	AUDIO_INITINFO(&info);
	info.play.sample_rate = pconf->sample_rate;
	info.play.channels = pconf->channels;
	info.play.precision = pconf->precision;
	info.play.encoding = AUDIO_ENCODING_SLINEAR_LE;
	info.record.sample_rate = rconf->sample_rate;
	info.record.channels = rconf->channels;
	info.record.precision = rconf->precision;
	info.record.encoding = AUDIO_ENCODING_SLINEAR_LE;
	info.mode = AUMODE_PLAY_ALL|AUMODE_RECORD;

	error = ioctl(fd, AUDIO_SETINFO, &info);
	if (error == -1)
		printf("AUDIO_SETINFO failed: %s\n", strerror(errno));

	return error;
}

int
thunk_audio_pollout(int fd)
{
	struct audio_info info;
	int error;

	AUDIO_INITINFO(&info);
	error = ioctl(fd, AUDIO_GETBUFINFO, &info);
	if (error == -1)
		return -1;

	return info.play.buffer_size - info.play.seek;
}

int
thunk_audio_pollin(int fd)
{
	struct audio_info info;
	int error;

	AUDIO_INITINFO(&info);
	error = ioctl(fd, AUDIO_GETBUFINFO, &info);
	if (error == -1)
		return -1;

	return info.record.seek;
}

ssize_t
thunk_audio_write(int fd, const void *buf, size_t buflen)
{
	return write(fd, buf, buflen);
}

ssize_t
thunk_audio_read(int fd, void *buf, size_t buflen)
{
	return read(fd, buf, buflen);
}

int
thunk_rfb_open(thunk_rfb_t *rfb, uint16_t port)
{
	struct sockaddr_in sin;
	int serrno;

	rfb->clientfd = -1;
	rfb->connected = false;

	/* create socket */
	rfb->sockfd = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (rfb->sockfd == -1) {
		serrno = errno;
		warn("rfb: couldn't create socket");
		return serrno;
	}
	/* bind to requested port */
	memset(&sin, 0, sizeof(sin));
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = htonl(INADDR_ANY);
	sin.sin_port = htons(port);
	if (bind(rfb->sockfd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
		serrno = errno;
		warn("rfb: couldn't bind port %d", port);
		close(rfb->sockfd);
		return serrno;
	}
	/* listen for connections */
	if (listen(rfb->sockfd, 1) != 0) {
		serrno = errno;
		warn("rfb: couldn't listen on socket");
		close(rfb->sockfd);
		return errno;
	}

	return 0;
}

int
thunk_gdb_open(void)
{
	struct sockaddr_in sin;
	int sockfd;
	int portnr = 5001;	/* XXX configurable or random */

	/* create socket */
	sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sockfd < 0) {
		warn("kgdb stub: couldn't create socket");
		return 0;
	}

	/* bind to requested port */
	memset(&sin, 0, sizeof(sin));
	sin.sin_family      = AF_INET;
	sin.sin_addr.s_addr = htonl(INADDR_ANY);
	sin.sin_port        = htons(portnr);

	if (bind(sockfd, (struct sockaddr *)&sin, sizeof(sin)) < 0) {
		warn("kgdb stub: couldn't bind port %d", portnr);
		close(sockfd);
		return 0;
	}

	/* listen for connections */
	if (listen(sockfd, 1) < 0) {
		warn("kgdb stub: couldn't listen on socket");
		close(sockfd);
		return 0;
	}
	printf("kgdb stub: accepting connections on port %d\n", portnr);

	return sockfd;
}

int
thunk_gdb_accept(int sockfd)
{
	struct sockaddr_in client_addr;
	socklen_t client_addrlen;
	int fd, flags;

	fd = accept(sockfd, (struct sockaddr *) &client_addr, &client_addrlen);
	if (fd < 0) {
		warn("kgdb_stub: connect error");
		return 0;
	}

  	/* make FIFO unblocking */
	flags = fcntl(fd, F_GETFL, 0);
	if (flags == -1)
		flags = 0;
	if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
		warn("kgdb_stub: can't make socket non blocking");
	}
	return fd;
}

int
thunk_kgdb_getc(int fd, char *ch)
{
	return safe_recv(fd, ch, 1);
}

int
thunk_kgdb_putc(int fd, char ch)
{
	return safe_send(fd, &ch, 1);
}

static ssize_t
safe_send(int s, const void *msg, int len)
{
	const uint8_t *p;
	ssize_t sent_len;

	p = msg;
	while (len) {
		assert(len >= 0);
		sent_len = send(s, p, len, MSG_NOSIGNAL);
		if (sent_len == -1) {
			if (errno == EAGAIN || errno == EINTR)
				continue;
			return -1;
		}

		p   += sent_len;
		len -= sent_len;
	}
	return len;
}

static ssize_t
safe_recv(int s, void *buf, int len)
{
	uint8_t *p;
	int recv_len;

	p = buf;
	while (len) {
		assert(len >= 0);
		recv_len = recv(s, p, len, MSG_NOSIGNAL);
		if (recv_len == -1)  {
			if (errno == EAGAIN || errno == EINTR)
				continue;
			return -1;
		}
		p   += recv_len;
		len -= recv_len;
	}
	return len;
}

static ssize_t
thunk_rfb_server_init(thunk_rfb_t *rfb)
{
	char msgbuf[80];
	char *p = msgbuf;
	uint32_t namelen = strlen(rfb->name);

	*(uint16_t *)p = htons(rfb->width);	p += 2;
	*(uint16_t *)p = htons(rfb->height);	p += 2;
	*(uint8_t *)p = rfb->depth;		p += 1;
	*(uint8_t *)p = rfb->depth;		p += 1;
	*(uint8_t *)p = 0;			p += 1;	/* endian */
	*(uint8_t *)p = 1;			p += 1; /* true color */
	*(uint16_t *)p = htons(0xff);		p += 2;	/* red max */
	*(uint16_t *)p = htons(0xff);		p += 2;	/* green max */
	*(uint16_t *)p = htons(0xff);		p += 2;	/* blue max */
	*(uint8_t *)p = 0;			p += 1;	/* red shift */
	*(uint8_t *)p = 8;			p += 1;	/* green shift */
	*(uint8_t *)p = 16;			p += 1;	/* blue shift */
	*(uint8_t *)p = 0;			p += 1;	/* padding x3 */
	*(uint8_t *)p = 0;			p += 1;
	*(uint8_t *)p = 0;			p += 1;
	*(uint32_t *)p = htonl(namelen);	p += 4;	/* name length */
	memcpy(p, rfb->name, namelen);		p += namelen;

	return safe_send(rfb->clientfd, msgbuf, p - msgbuf);
}

static int
thunk_rfb_handshake(thunk_rfb_t *rfb)
{
	ssize_t len;
	const char *protover = "RFB 003.003\n";
	uint32_t security_type;
	uint8_t shared_flag;
	char dummy;

	/* send server protocol version */
	len = safe_send(rfb->clientfd, protover, strlen(protover));
	if (len == -1)
		return errno;

	/* receive client protocol version */
	do {
		len = safe_recv(rfb->clientfd, &dummy, sizeof(dummy));
		if (len == -1)
			return errno;
	} while (dummy != '\n');

	/* send security capabilities */
	security_type = htonl(1);	/* no security */
	len = safe_send(rfb->clientfd, &security_type, sizeof(security_type));
	if (len == -1)
		return errno;

	/* receive client init message */
	len = safe_recv(rfb->clientfd, &shared_flag, sizeof(shared_flag));
	if (len == -1)
		return errno;

	/* send server init message */
	len = thunk_rfb_server_init(rfb);
	if (len == -1)
		return errno;

	return 0;
}

static void
thunk_rfb_send_pending(thunk_rfb_t *rfb)
{
	thunk_rfb_update_t *update;
	uint8_t buf[32];
	uint8_t *p;
	unsigned int n;
	unsigned int bytes_per_pixel;
	ssize_t stride, line_len, len;

	if (rfb->connected == false || rfb->nupdates == 0)
		return;

	/* If we have too many updates queued, just send a single update */
	if (rfb->nupdates >= __arraycount(rfb->update)) {
		rfb->nupdates = 1;
		rfb->update[0].enc = THUNK_RFB_TYPE_RAW;
		rfb->update[0].x = 0;
		rfb->update[0].y = 0;
		rfb->update[0].w = rfb->width;
		rfb->update[0].h = rfb->height;
	}

#ifdef RFB_DEBUG
	fprintf(stdout, "rfb: sending %d updates\n", rfb->nupdates);
#endif

	p = buf;
	*(uint8_t *)p = 0;		p += 1;		/* FramebufferUpdate */
	*(uint8_t *)p = 0;		p += 1;		/* padding */
	*(uint16_t *)p = htons(rfb->nupdates);	p += 2;	/* # rects */

	len = safe_send(rfb->clientfd, buf, 4);
	if (len == -1)
		goto disco;

	bytes_per_pixel = rfb->depth / 8;
	stride = rfb->width * bytes_per_pixel;
	for (n = 0; n < rfb->nupdates; n++) {
		p = buf;
		update = &rfb->update[n];
		*(uint16_t *)p = htons(update->x);	p += 2;
		*(uint16_t *)p = htons(update->y);	p += 2;
		*(uint16_t *)p = htons(update->w);	p += 2;
		*(uint16_t *)p = htons(update->h);	p += 2;
		*(uint32_t *)p = htonl(update->enc);	p += 4;	/* encoding */

#ifdef RFB_DEBUG
		fprintf(stdout, "rfb: [%u] enc %d, [%d, %d] - [%d, %d]",
		    n, update->enc, update->x, update->y, update->w, update->h);
		if (update->enc == THUNK_RFB_TYPE_COPYRECT)
			fprintf(stdout, " from [%d, %d]",
			    update->srcx, update->srcy);
		if (update->enc == THUNK_RFB_TYPE_RRE)
			fprintf(stdout, " pixel [%02x %02x %02x %02x]",
			    update->pixel[0], update->pixel[1],
			    update->pixel[2], update->pixel[3]);
		fprintf(stdout, "\n");
#endif

		len = safe_send(rfb->clientfd, buf, 12);
		if (len == -1)
			goto disco;

		if (update->enc == THUNK_RFB_TYPE_COPYRECT) {
			p = buf;
			*(uint16_t *)p = htons(update->srcx);	p += 2;
			*(uint16_t *)p = htons(update->srcy);	p += 2;
			len = safe_send(rfb->clientfd, buf, 4);
			if (len == -1)
				goto disco;
		}

		if (update->enc == THUNK_RFB_TYPE_RRE) {
			p = buf;

			/* header */
			*(uint32_t *)p = htonl(1);		p += 4;
			memcpy(p, update->pixel, 4);		p += 4;
			/* subrectangle */
			memcpy(p, update->pixel, 4);		p += 4;
			*(uint16_t *)p = htons(update->x);	p += 2;
			*(uint16_t *)p = htons(update->y);	p += 2;
			*(uint16_t *)p = htons(update->w);	p += 2;
			*(uint16_t *)p = htons(update->h);	p += 2;
			/* send it */
			len = safe_send(rfb->clientfd, buf, 20);
			if (len == -1)
				goto disco;
		}

		if (update->enc == THUNK_RFB_TYPE_RAW) {
			p = rfb->framebuf + (update->y * stride)
				+ (update->x * bytes_per_pixel);
			line_len = update->w * bytes_per_pixel;
			while (update->h-- > 0) {
				len = safe_send(rfb->clientfd, p, line_len);
				if (len == -1)
					goto disco;
				p += stride;
			}
		}
	}

	rfb->nupdates = 0;
	rfb->first_mergable = 0;

	return;

disco:
	fprintf(stdout, "rfb: client disconnected: %s\n", strerror(errno));
	close(rfb->clientfd);
	rfb->clientfd = -1;
	rfb->connected = false;
}

int
thunk_rfb_poll(thunk_rfb_t *rfb, thunk_rfb_event_t *event)
{
	int error, len, msg_len;
	uint8_t set_pixel_format[19];
	uint8_t set_encodings[3];
	uint8_t framebuffer_update_request[9];
	uint8_t key_event[7];
	uint8_t pointer_event[5];
	uint8_t client_cut_text[7];
	uint8_t ch;

	if (rfb->clientfd == -1) {
		struct sockaddr_in sin;
		struct pollfd fds[1];
		socklen_t sinlen;
		int flags;

#ifdef RFB_DEBUG
		fprintf(stdout, "rfb: poll connection\n");
#endif

		/* poll for connections */
		fds[0].fd = rfb->sockfd;
		fds[0].events = POLLIN;
		fds[0].revents = 0;
		if (poll(fds, __arraycount(fds), 0) != 1) {
#ifdef RFB_DEBUG
			fprintf(stdout, "rfb: NO connection\n");
#endif
			return -1;
		}

#ifdef RFB_DEBUG
		fprintf(stdout, "rfb: try accept\n");
#endif

		sinlen = sizeof(sin);
		rfb->clientfd = accept(rfb->sockfd, (struct sockaddr *)&sin,
		    &sinlen);
		if (rfb->clientfd == -1)
			return -1;

		fprintf(stdout, "rfb: connection from %s:%d\n",
		    inet_ntoa(sin.sin_addr), ntohs(sin.sin_port));

		/* rfb handshake */
		if (thunk_rfb_handshake(rfb) != 0) {
			fprintf(stdout, "rfb: handshake failed\n");
			close(rfb->clientfd);
			rfb->clientfd = -1;
			return -1;
		}

		rfb->connected = true;

		/* enable sigio on input */
		flags = fcntl(rfb->clientfd, F_GETFL, 0);
		fcntl(rfb->clientfd, F_SETFL, flags | O_ASYNC);
		error = fcntl(rfb->clientfd, F_SETOWN, getpid());
		if (error) {
			fprintf(stdout, "rfb: setown failed: %s\n",
			    strerror(errno));
			close(rfb->clientfd);
			rfb->clientfd = -1;
			return -1;
		}

		rfb->schedule_bell = false;
		rfb->nupdates = 0;
		rfb->first_mergable = 0;
		thunk_rfb_update(rfb, 0, 0, rfb->width, rfb->height);
	}

	thunk_rfb_send_pending(rfb);

	if (rfb->clientfd == -1)
		return -1;

	if (event == NULL)
		return 0;

	if (rfb->schedule_bell) {
		uint8_t msg_type = 2;	/* bell */
		safe_send(rfb->clientfd, &msg_type, sizeof(msg_type));
		rfb->schedule_bell = false;
	}

	error = ioctl(rfb->clientfd, FIONREAD, &len);
	if (error == -1)
		goto discon;
	if (len == 0)
		return 0;

	len = safe_recv(rfb->clientfd, &ch, sizeof(ch));
	if (len == -1)
		goto discon;

	event->message_type = ch;
	switch (ch) {
	case THUNK_RFB_SET_PIXEL_FORMAT:
		msg_len = sizeof(set_pixel_format);
		break;
	case THUNK_RFB_SET_ENCODINGS:
		len = safe_recv(rfb->clientfd,
			set_encodings, sizeof(set_encodings));
		if (len == -1)
			goto discon;
		msg_len = 4 * ntohs(*(uint16_t *)&set_encodings[1]);
		break;
	case THUNK_RFB_FRAMEBUFFER_UPDATE_REQUEST:
		len = safe_recv(rfb->clientfd,
			framebuffer_update_request,
			sizeof(framebuffer_update_request));
		if (len == -1)
			goto discon;
#ifdef RFB_DEBUG
		fprintf(stdout, "framebuffer update request: ");
		fprintf(stdout, "[%d, %d] + [%d, %d] %s\n",
			framebuffer_update_request[1],
			framebuffer_update_request[2],
			framebuffer_update_request[3],
			framebuffer_update_request[4],
			framebuffer_update_request[0]?"Incrmental":"Complete");
#endif

		if (framebuffer_update_request[0] == 0) {
			/* complete redraw request -> buffer full */
			rfb->nupdates = __arraycount(rfb->update) + 1;
		}
//		thunk_rfb_send_pending(rfb);
		msg_len = 0;
		break;
	case THUNK_RFB_KEY_EVENT:
		len = safe_recv(rfb->clientfd, key_event, sizeof(key_event));
		if (len == -1)
			goto discon;
		event->data.key_event.down_flag = key_event[0];
		event->data.key_event.keysym =
		    ntohl(*(uint32_t *)&key_event[3]);
#ifdef RFB_DEBUG
		fprintf(stdout, "rfb: key %04x %s\n",
		    event->data.key_event.keysym,
		    event->data.key_event.down_flag ? "pressed" : "released");
#endif
		msg_len = 0;
		break;
	case THUNK_RFB_POINTER_EVENT:
		len = safe_recv(rfb->clientfd,
			pointer_event, sizeof(pointer_event));
		if (len == -1)
			goto discon;
		event->data.pointer_event.button_mask = pointer_event[0];
		event->data.pointer_event.absx =
		    ntohs(*(uint16_t *)&pointer_event[1]);
		event->data.pointer_event.absy =
		    ntohs(*(uint16_t *)&pointer_event[3]);
#ifdef RFB_DEBUG
		fprintf(stdout, "rfb: pointer mask %02x abs %dx%d\n",
		    event->data.pointer_event.button_mask,
		    event->data.pointer_event.absx,
		    event->data.pointer_event.absy);
#endif
		msg_len = 0;
		break;
	case THUNK_RFB_CLIENT_CUT_TEXT:
		len = safe_recv(rfb->clientfd,
			client_cut_text, sizeof(client_cut_text));
		if (len == -1)
			goto discon;
		msg_len = ntohl(*(uint32_t *)&client_cut_text[3]);
		break;
	default:
		fprintf(stdout, "rfb: unknown message type %d\n", ch);
		goto discon;
	}

	if (len == -1)
		goto discon;

	/* discard any remaining bytes */
	while (msg_len-- > 0) {
		len = safe_recv(rfb->clientfd, &ch, sizeof(ch));
		if (len == -1)
			goto discon;
	}

	return 1;

discon:
	//printf("rfb: safe_recv failed: %s\n", strerror(errno));
	close(rfb->clientfd);
	rfb->clientfd = -1;

	return -1;
}

void
thunk_rfb_update(thunk_rfb_t *rfb, int x, int y, int w, int h)
{
	thunk_rfb_update_t *update = NULL;
	unsigned int n;

	/* if the queue is full, just return */
	if (rfb->nupdates >= __arraycount(rfb->update))
		return;

	/* no sense in queueing duplicate updates */
	for (n = rfb->first_mergable; n < rfb->nupdates; n++) {
		if (rfb->update[n].x == x && rfb->update[n].y == y &&
		    rfb->update[n].w == w && rfb->update[n].h == h)
			return;
	}

#ifdef RFB_DEBUG
	fprintf(stdout, "rfb: update queue slot %d, x=%d y=%d w=%d h=%d\n",
	    rfb->nupdates, x, y, w, h);
#endif

	/* add the update request to the queue */
	update = &rfb->update[rfb->nupdates++];
	update->enc = THUNK_RFB_TYPE_RAW;
	update->x = x;
	update->y = y;
	update->w = w;
	update->h = h;
}

void
thunk_rfb_bell(thunk_rfb_t *rfb)
{
#ifdef RFB_DEBUG
	fprintf(stdout, "rfb: schedule bell\n");
#endif
	rfb->schedule_bell = true;
}

void
thunk_rfb_copyrect(thunk_rfb_t *rfb, int x, int y, int w, int h,
	int srcx, int srcy)
{
	thunk_rfb_update_t *update = NULL;

	/* if the queue is full, just return */
	if (rfb->nupdates >= __arraycount(rfb->update))
		return;

#ifdef RFB_DEBUG
	fprintf(stdout, "rfb: copyrect queue slot %d, x=%d y=%d w=%d h=%d\n",
	    rfb->nupdates, x, y, w, h);
#endif

	/* add the update request to the queue */
	update = &rfb->update[rfb->nupdates++];
	update->enc = THUNK_RFB_TYPE_COPYRECT;
	update->x = x;
	update->y = y;
	update->w = w;
	update->h = h;
	update->srcx = srcx;
	update->srcy = srcy;

	rfb->first_mergable = rfb->nupdates;
}

void
thunk_rfb_fillrect(thunk_rfb_t *rfb, int x, int y, int w, int h, uint8_t *pixel)
{
	thunk_rfb_update_t *update = NULL;

	/* if the queue is full, just return */
	if (rfb->nupdates >= __arraycount(rfb->update))
		return;

#ifdef RFB_DEBUG
	fprintf(stdout, "rfb: fillrect queue slot %d, x=%d y=%d w=%d h=%d\n",
	    rfb->nupdates, x, y, w, h);
#endif

	/* add the update request to the queue */
	update = &rfb->update[rfb->nupdates++];
	update->enc = THUNK_RFB_TYPE_RRE;
	update->x = x;
	update->y = y;
	update->w = w;
	update->h = h;
	memcpy(update->pixel, pixel, 4);

	rfb->first_mergable = rfb->nupdates;
}