xref: /netbsd/src/sys/arch/usermode/usermode/thunk.c

/* $NetBSD: thunk.c,v 1.93 2024/03/17 21:48:01 andvar 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.93 2024/03/17 21:48:01 andvar 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]?"Incremental":"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;
}