#include <aio.h>
#include <sha.h>
#include <arc4.h>
#include <stdio.h>
#include <errno.h>
#include <ctype.h>
#include <fcntl.h>
#include <netdb.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdarg.h>
#include <signal.h>
#include <net/if.h>
#include <strings.h>
#include <rijndael.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <sys/ioctl.h>
#include <arpa/inet.h>
#include <net/if_tap.h>
#include <netinet/in.h>
#include <sys/timersock.h>
//#include <net/if_bridgevar.h>

extern const char *__progname;

#define REKEY_INTERVAL 8192
//#define REKEY_INTERVAL 0x10000000

static const char *conf_path = "/root/iq4b-vpn/config";
static const char *random_path = "/dev/urandom";

static const char *devnull_path = "/dev/null";
static const char *tap_path = "/dev/tap";

#include "es.h"

typedef struct config CONFIG;
typedef struct client CLIENT;
typedef struct listen LISTEN;
typedef struct cparse CPARSE;
typedef struct conn CONN;
typedef struct cstate CSTATE;
typedef struct cflow CFLOW;

/*
 * ke is 33, not 32, to simplify the logic surrounding crypto_rekey_r
 *  (this way we don't need yet another state representing "waiting for
 *  R" in addition to "waiting for KE").
 */
struct cflow {
  unsigned int until_rekey;
  unsigned char kc[32];
  unsigned char ke[33];
  unsigned char r;
  void *rj;
  ARC4_STATE enc;
  } ;

struct cstate {
  unsigned char cmk[32];
  unsigned char smk[32];
  CFLOW r;
  CFLOW w;
  } ;

/*
 * step is 1 when reading the client's initial nonce.
 * step is 2 when awaiting the client's verifier.
 * step is 3 when awaiting the client's echo of our verifier.
 * step is 0 when it's gone live.
 */
struct conn {
  int inlist;
  CONN *flink;
  CONN *blink;
  char *text;
  int fd;
  int pid;
  int bid;
  LISTEN *l;
  AIO_OQ oq;
  int step;
  int fill;
  unsigned char cn[16];
  unsigned char sn[16];
  unsigned char cv[16];
  unsigned char vsend[16];
  unsigned char vrecv[16];
  CSTATE cs;
  unsigned char packet[65538];
  } ;

struct listen {
  LISTEN *link;
  CLIENT *client;
  char *text;
  struct sockaddr *sa;
  int salen;
  int afd;
  int aid;
  CONN *new;
  } ;

struct client {
  CLIENT *link;
  CONFIG *config;
  char *name;
  LISTEN *listen;
  struct in_addr *baseip;
  char *keyfile;
  unsigned char *secret;
  int secretlen;
  CONN *live;
  int tapfd;
  int tapid;
  char *tapname;
  } ;

struct config {
  CLIENT *clients;
  CPARSE *p;
  } ;

struct cparse {
  int error;
  CLIENT *cur;
  const unsigned char *line;
  int lno;
  } ;

static CONFIG *conf;
static int devnull;
static int randfd;
static int randtweak;
static int randtweak_fd;
static int randid;
static int ifsock;
static unsigned int randpool[64];
static int randptr;
static int verbose;
static int verbpipe[2];

#define Cisspace(x) isspace((unsigned char)(x))

static void handleargs(int ac, char **av)
{
 int skip;
 int errs;

 skip = 0;
 errs = 0;
 for (ac--,av++;ac;ac--,av++)
  { if (skip > 0)
     { skip --;
       continue;
     }
    if (**av != '-')
     { fprintf(stderr,"%s: stray argument `%s'\n",__progname,*av);
       errs = 1;
       continue;
     }
    if (0)
     {
needarg:;
       fprintf(stderr,"%s: %s needs a following argument\n",__progname,*av);
       errs = 1;
       continue;
     }
#define WANTARG() do { if (++skip >= ac) goto needarg; } while (0)
    if (!strcmp(*av,"-config"))
     { WANTARG();
       conf_path = av[skip];
       continue;
     }
#undef WANTARG
    fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av);
    errs = 1;
  }
 if (errs) exit(1);
}

static CLIENT *client_new(CONFIG *cf)
{
 CLIENT *c;

 c = malloc(sizeof(CLIENT));
 c->config = cf;
 c->name = 0;
 c->listen = 0;
 c->baseip = 0;
 c->keyfile = 0;
 c->secret = 0;
 c->secretlen = 0;
 c->live = 0;
 c->tapfd = -1;
 c->tapid = AIO_NOID;
 c->tapname = 0;
 return(c);
}

static CONFIG *config_new(void)
{
 CONFIG *c;

 c = malloc(sizeof(CONFIG));
 c->clients = 0;
 c->p = 0;
 return(c);
}

static void config_start_cur(CONFIG *cf)
{
 CLIENT *cl;

 if (! cf->p->cur)
  { cl = client_new(cf);
    cl->link = cf->clients;
    cf->clients = cl;
    cf->p->cur = cl;
  }
}

static void config_err(CONFIG *, const char *, ...)
	__attribute__((__format__(__printf__,2,3)));
static void config_err(CONFIG *cf, const char *fmt, ...)
{
 va_list ap;
 char *s;

 va_start(ap,fmt);
 vasprintf(&s,fmt,ap);
 va_end(ap);
 fprintf(stderr,"%s: %s, line %d: %s\n  line: %s\n",__progname,conf_path,cf->p->lno,s,cf->p->line);
 free(s);
 cf->p->error = 1;
}

static void config_load_line_(CONFIG *cf)
{
 cf->p->cur = 0;
}

static void config_load_line_key(CONFIG *cf, const unsigned char *body)
{
 int fd;
 struct stat stb;
 int n;

 config_start_cur(cf);
 if (cf->p->cur->keyfile)
  { config_err(cf,"keyfile specified multiple times");
    return;
  }
 cf->p->cur->keyfile = strdup(body);
 fd = open(body,O_RDONLY,0);
 if (fd < 0)
  { config_err(cf,"can't open keyfile %s: %s",body,strerror(errno));
    return;
  }
 if (fstat(fd,&stb) < 0)
  { config_err(cf,"can't fstat keyfile %s: %s",body,strerror(errno));
    close(fd);
    return;
  }
 if (stb.st_size > 65536)
  { config_err(cf,"keyfile %s too large",body);
    close(fd);
    return;
  }
 cf->p->cur->secretlen = stb.st_size;
 cf->p->cur->secret = malloc(stb.st_size);
 n = read(fd,cf->p->cur->secret,cf->p->cur->secretlen);
 if (n < 0)
  { config_err(cf,"read error on keyfile %s: %s",body,strerror(errno));
    close(fd);
    return;
  }
 if (n != cf->p->cur->secretlen)
  { config_err(cf,"reading keyfile %s expected %d, got %d",body,cf->p->cur->secretlen,n);
    close(fd);
    return;
  }
 close(fd);
}

static void config_load_line_base(CONFIG *cf, const unsigned char *body)
{
 int r;
 struct in_addr a;

 config_start_cur(cf);
 r = inet_pton(AF_INET,body,&a);
 switch (r)
  { case 1:
       break;
    case 0:
       config_err(cf,"invalid IPv4 address");
       return;
       break;
    case -1:
       config_err(cf,"error parsing IPv4 address: %s",strerror(errno));
       return;
       break;
    default:
       config_err(cf,"unexpected return %d from inet_pton",r);
       return;
       break;
  }
 if (cf->p->cur->baseip)
  { config_err(cf,"base IP specified multiple times");
    return;
  }
 if (((unsigned char *)&a)[3] & 0x03)
  { config_err(cf,"base IP low two bits are 0");
    return;
  }
 cf->p->cur->baseip = malloc(sizeof(struct in_addr));
 *cf->p->cur->baseip = a;
}

static void config_load_line_client(CONFIG *cf, const unsigned char *body)
{
 config_start_cur(cf);
 if (cf->p->cur->name)
  { config_err(cf,"name specified multiple times");
    return;
  }
 cf->p->cur->name = strdup(body);
}

static void config_load_line_listen(CONFIG *cf, const unsigned char *body)
{
 char *slash;
 char *hoststr;
 char *portstr;
 int i;
 struct addrinfo *ai0;
 struct addrinfo *ai;
 struct addrinfo hints;
 LISTEN *l;
 char gnihost[NI_MAXHOST];
 char gniserv[NI_MAXSERV];

 config_start_cur(cf);
 slash = index(body,'/');
 if (! slash)
  { config_err(cf,"no slash in listen address");
    return;
  }
 i = slash - (const char *)body;
 hoststr = malloc(i+1);
 bcopy(body,hoststr,i);
 hoststr[i] = '\0';
 portstr = strdup(slash+1);
 do
  { hints.ai_flags = AI_PASSIVE;
    hints.ai_family = PF_UNSPEC;
    hints.ai_socktype = SOCK_STREAM;
    hints.ai_protocol = 0;
    hints.ai_addrlen = 0; // XXX API botch
    hints.ai_canonname = 0; // XXX API botch
    hints.ai_addr = 0; // XXX API botch
    hints.ai_next = 0; // XXX API botch
    i = getaddrinfo(hoststr,portstr,&hints,&ai0);
    if (i)
     { config_err(cf,"can't resolve `%s': %s",body,gai_strerror(i));
       break;
     }
    if (! ai0)
     { config_err(cf,"resolution succeeded with zero addresses for `%s'",body);
       break;
     }
    for (ai=ai0;ai;ai=ai->ai_next)
     { i = getnameinfo(ai->ai_addr,ai->ai_addrlen,&gnihost[0],NI_MAXHOST,&gniserv[0],NI_MAXSERV,NI_NUMERICHOST|NI_NUMERICSERV);
       if (i)
	{ config_err(cf,"getnameinfo failed for `%s': %s",body,gai_strerror(i));
	  break;
	}
       l = malloc(sizeof(LISTEN));
       l->client = cf->p->cur;
       l->salen = ai->ai_addrlen;
       l->sa = malloc(l->salen);
       bcopy(ai->ai_addr,l->sa,l->salen);
       asprintf(&l->text,"%s/%s",&gnihost[0],&gniserv[0]);
       l->afd = -1;
       l->aid = AIO_NOID;
       l->new = 0;
       l->link = cf->p->cur->listen;
       cf->p->cur->listen = l;
     }
    freeaddrinfo(ai0);
  } while (0);
 free(hoststr);
 free(portstr);
}

static void config_load_line(CONFIG *c, const unsigned char *l)
{
 int x;
 int x0;
 int kl;

 if (! c->p)
  { c->p = malloc(sizeof(CPARSE));
    c->p->error = 0;
    c->p->cur = 0;
    c->p->line = 0;
    c->p->lno = 0;
  }
 if (c->p->error) return;
 c->p->lno ++;
 c->p->line = l;
 for (x=0;l[x]&&Cisspace(l[x]);x++) ;
 switch (l[x])
  { case '\0':
       config_load_line_(c);
       return;
       break;
    case '#':
    case ';':
       return;
       break;
  }
 x0 = x;
 for (;l[x]&&!Cisspace(l[x]);x++) ;
 kl = x - x0;
 for (;l[x]&&Cisspace(l[x]);x++) ;
 switch (kl)
  { case 3:
       if (!bcmp(l+x0,"key",3))
	{ config_load_line_key(c,l+x);
	  return;
	}
       break;
    case 4:
       if (!bcmp(l+x0,"base",4))
	{ config_load_line_base(c,l+x);
	  return;
	}
       break;
    case 6:
       switch (l[x0])
	{ case 'c':
	     if (!bcmp(l+x0,"client",6))
	      { config_load_line_client(c,l+x);
		return;
	      }
	     break;
	  case 'l':
	     if (!bcmp(l+x0,"listen",6))
	      { config_load_line_listen(c,l+x);
		return;
	      }
	     break;
	}
       break;
  }
 config_err(c,"unrecognized config line keyword `%.*s'",kl,l+x0);
 c->p->error = 1;
}

static void config_load_final(CONFIG *cf)
{
 if (! cf->p) return;
 if (cf->p->error) abort();
 free(cf->p);
 cf->p = 0;
}

static void conn_free(CONN *c)
{
 if (c->fd >= 0) close(c->fd);
 if (c->pid != AIO_NOID) aio_remove_poll(c->pid);
 if (c->bid != AIO_NOID) aio_remove_block(c->bid);
 aio_oq_flush(&c->oq);
 rijndael_done(c->cs.r.rj);
 rijndael_done(c->cs.w.rj);
 free(c->text);
 free(c);
}

static void listen_free(LISTEN *l)
{
 CONN *c;

 if (l->aid != AIO_NOID) aio_remove_poll(l->aid);
 while ((c = l->new))
  { l->new = c->flink;
    conn_free(c);
  }
 free(l->text);
 free(l->sa);
 free(l);
}

static void client_nolive(CLIENT *c)
{
 if (c->tapfd >= 0) close(c->tapfd);
 c->tapfd = -1;
 if (c->tapid != AIO_NOID) aio_remove_poll(c->tapid);
 c->tapid = AIO_NOID;
 free(c->tapname);
 c->tapname = 0;
 c->live = 0;
}

static void client_free(CLIENT *c)
{
 LISTEN *l;

 client_nolive(c);
 while ((l = c->listen))
  { c->listen = l->link;
    listen_free(l);
  }
 free(c->name);
 free(c->baseip);
 free(c->keyfile);
 free(c->secret);
 free(c);
}

static void config_free(CONFIG *cf)
{
 CLIENT *cl;

 while ((cl = cf->clients))
  { cf->clients = cl->link;
    client_free(cl);
  }
 free(cf->p);
 free(cf);
}

static CONFIG *config_load(void)
{
 FILE *f;
 ES line;
 int c;
 CONFIG *cf;

 f = fopen(conf_path,"r");
 if (f == 0)
  { fprintf(stderr,"%s: can't open config file %s: %s\n",__progname,conf_path,strerror(errno));
    exit(1);
  }
 cf = config_new();
 es_init(&line);
 while (1)
  { c = getc(f);
    if (c == EOF) break;
    if (c == '\n')
     { es_append_1(&line,'\0');
       config_load_line(cf,es_buf(&line));
       es_clear(&line);
     }
    else
     { es_append_1(&line,c);
     }
  }
 if (es_len(&line))
  { fprintf(stderr,"%s: config file %s: apparently truncated (missing final newline)\n",__progname,conf_path);
    exit(1);
  }
 if (cf->p && cf->p->error)
  { config_free(cf);
    return(0);
  }
 config_load_final(cf);
 return(cf);
}

static void random_stir(void)
{
 unsigned char *pp;
 int o;
 void *h;
 unsigned char t;
 unsigned char hr[64];
 int i;
 int j;

 pp = (void *)&randpool;
 o = 0;
 if (randtweak)
  { randtweak = 0;
    read(randfd,&t,1);
  }
 while (1)
  { if (o >= sizeof(randpool)) break;
    h = sha512_init();
    sha512_process_bytes(h,&t,1);
    sha512_process_bytes(h,&randpool,sizeof(randpool));
    sha512_process_bytes(h,&t,1);
    sha512_result(h,&hr[0]);
    i = sizeof(randpool) - o;
    if (i > 64) i = 64;
    for (j=i-1;j>=0;j--) ((unsigned char *)&randpool)[o+j] ^= hr[j];
    o += 64;
  }
 randptr = sizeof(randpool) / 2;
}

static void random_bytes(void *data, int len)
{
 unsigned char *dp;

 dp = data;
 for (;len>0;len--)
  { if (randptr < 1) random_stir();
    *dp++ = ((unsigned char *)&randpool)[--randptr];
  }
}

static void dump_data(FILE *to, const void *data, int len)
{
 const unsigned char *dp;
 int o;

 dp = data;
 for (o=0;o<len;o++)
  { switch (o & 15)
     { case 0: fprintf(to,"%04x: ",o); break;
       case 8: fprintf(to," "); break;
     }
    fprintf(to," %02x",dp[o]);
    switch (o & 15)
     { case 15: fprintf(to,"\n"); break;
     }
  }
 if (len & 15) fprintf(to,"\n");
}

static void queue_encrypted(AIO_OQ *oq, ARC4_STATE *enc, const void *data, int len)
{
 const unsigned char *dp;
 unsigned char buf[32];

 dp = data;
 if (verbose)
  { printf("queue_encrypted %d:\n",len);
    dump_data(stdout,data,len);
  }
 while (len >= sizeof(buf))
  { arc4_crypt(enc,dp,sizeof(buf),&buf[0]);
    aio_oq_queue_copy(oq,&buf[0],sizeof(buf));
    dp += sizeof(buf);
    len -= sizeof(buf);
  }
 if (len)
  { arc4_crypt(enc,dp,len,&buf[0]);
    aio_oq_queue_copy(oq,&buf[0],len);
  }
}

static void crypto_nonce(void *data, int len)
{
 random_bytes(data,len);
}

static void crypto_rekey_w(CSTATE *s, AIO_OQ *oq)
{
 crypto_nonce(&s->w.kc[0],32);
 if (verbose)
  { printf("write rekey\n");
    printf("kc %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	s->w.kc[0],s->w.kc[1],s->w.kc[2],s->w.kc[3],s->w.kc[4],s->w.kc[5],s->w.kc[6],s->w.kc[7],
	s->w.kc[8],s->w.kc[9],s->w.kc[10],s->w.kc[11],s->w.kc[12],s->w.kc[13],s->w.kc[14],s->w.kc[15],
	s->w.kc[16],s->w.kc[17],s->w.kc[18],s->w.kc[19],s->w.kc[20],s->w.kc[21],s->w.kc[22],s->w.kc[23],
	s->w.kc[24],s->w.kc[25],s->w.kc[26],s->w.kc[27],s->w.kc[28],s->w.kc[29],s->w.kc[30],s->w.kc[31]);
  }
 rijndael_set_key(s->w.rj,&s->smk[0]);
 rijndael_encrypt(s->w.rj,&s->w.kc[0],&s->w.ke[0]);
 if (verbose)
  { printf("ke %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	s->w.ke[0],s->w.ke[1],s->w.ke[2],s->w.ke[3],s->w.ke[4],s->w.ke[5],s->w.ke[6],s->w.ke[7],
	s->w.ke[8],s->w.ke[9],s->w.ke[10],s->w.ke[11],s->w.ke[12],s->w.ke[13],s->w.ke[14],s->w.ke[15],
	s->w.ke[16],s->w.ke[17],s->w.ke[18],s->w.ke[19],s->w.ke[20],s->w.ke[21],s->w.ke[22],s->w.ke[23],
	s->w.ke[24],s->w.ke[25],s->w.ke[26],s->w.ke[27],s->w.ke[28],s->w.ke[29],s->w.ke[30],s->w.ke[31]);
  }
 aio_oq_queue_copy(oq,&s->w.ke[0],32);
 arc4_setkey(&s->w.enc,&s->w.kc[0],31,65536+s->w.kc[31]);
 queue_encrypted(oq,&s->w.enc,&s->w.r,1);
 s->w.until_rekey = REKEY_INTERVAL + s->w.r;
}

static void crypto_rekey_r(CSTATE *s)
{
 if (verbose) printf("read rekey\n");
 rijndael_set_key(s->r.rj,&s->cmk[0]);
 if (verbose) printf("ke %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	s->r.ke[0],s->r.ke[1],s->r.ke[2],s->r.ke[3],s->r.ke[4],s->r.ke[5],s->r.ke[6],s->r.ke[7],
	s->r.ke[8],s->r.ke[9],s->r.ke[10],s->r.ke[11],s->r.ke[12],s->r.ke[13],s->r.ke[14],s->r.ke[15],
	s->r.ke[16],s->r.ke[17],s->r.ke[18],s->r.ke[19],s->r.ke[20],s->r.ke[21],s->r.ke[22],s->r.ke[23],
	s->r.ke[24],s->r.ke[25],s->r.ke[26],s->r.ke[27],s->r.ke[28],s->r.ke[29],s->r.ke[30],s->r.ke[31]);
 rijndael_decrypt(s->r.rj,&s->r.ke[0],&s->r.kc[0]);
 if (verbose) printf("kc %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	s->r.kc[0],s->r.kc[1],s->r.kc[2],s->r.kc[3],s->r.kc[4],s->r.kc[5],s->r.kc[6],s->r.kc[7],
	s->r.kc[8],s->r.kc[9],s->r.kc[10],s->r.kc[11],s->r.kc[12],s->r.kc[13],s->r.kc[14],s->r.kc[15],
	s->r.kc[16],s->r.kc[17],s->r.kc[18],s->r.kc[19],s->r.kc[20],s->r.kc[21],s->r.kc[22],s->r.kc[23],
	s->r.kc[24],s->r.kc[25],s->r.kc[26],s->r.kc[27],s->r.kc[28],s->r.kc[29],s->r.kc[30],s->r.kc[31]);
 arc4_setkey(&s->r.enc,&s->r.kc[0],31,65536+s->r.kc[31]);
 arc4_crypt(&s->r.enc,&s->r.kc[31],1,&s->r.r);
 s->r.until_rekey = REKEY_INTERVAL + s->r.r;
}

static void crypto_gen_mk(unsigned char *cn, unsigned char *sn, const void *ss, int sslen, const char *x2y, int ibase, unsigned char *mk)
{
 void *hh;
 unsigned char ib;
 int i;
 const unsigned char *t;
 int tl;
 unsigned char hbuf[64];

 t = ss;
 tl = sslen;
 for (i=0;i<64;i++)
  { ib = ibase + i;
    hh = sha512_init();
    sha512_process_bytes(hh,ss,sslen);
    sha512_process_bytes(hh,t,tl);
    sha512_process_bytes(hh,&ib,1);
    sha512_process_bytes(hh,x2y,4);
    sha512_process_bytes(hh,sn,16);
    sha512_process_bytes(hh,cn,16);
    sha512_process_bytes(hh,t,tl);
    sha512_process_bytes(hh,ss,sslen);
    sha512_result(hh,&hbuf[0]);
    t = &hbuf[0];
    tl = 64;
  }
 for (i=0;i<32;i++) mk[i] = t[i] ^ t[63-i];
}

static void crypto_gen_mks(unsigned char *cn, unsigned char *sn, const void *ss, int sslen, unsigned char *smk, unsigned char *cmk)
{
 if (verbose)
  { int i;
    printf("gen_mks\n");
    printf("sn = %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	sn[0],sn[1],sn[2],sn[3],sn[4],sn[5],sn[6],sn[7],
	sn[8],sn[9],sn[10],sn[11],sn[12],sn[13],sn[14],sn[15]);
    printf("cn = %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	cn[0],cn[1],cn[2],cn[3],cn[4],cn[5],cn[6],cn[7],
	cn[8],cn[9],cn[10],cn[11],cn[12],cn[13],cn[14],cn[15]);
    printf("ss =");
    for (i=0;i<sslen;i++) printf(" %02x",((const unsigned char *)ss)[i]);
    printf("\n");
  }
 crypto_gen_mk(cn,sn,ss,sslen,"\x63\x2d\x3e\x73",0,cmk);
 crypto_gen_mk(cn,sn,ss,sslen,"\x73\x2d\x3e\x63",64,smk);
}

static void queue_data(AIO_OQ *oq, CSTATE *s, const void *data, int len)
{
 if (len < s->w.until_rekey)
  { queue_encrypted(oq,&s->w.enc,data,len);
    s->w.until_rekey -= len;
    return;
  }
 queue_encrypted(oq,&s->w.enc,data,s->w.until_rekey);
 len -= s->w.until_rekey;
 data = s->w.until_rekey + (const char *)data;
 crypto_rekey_w(s,oq);
 if (len >= s->w.until_rekey) abort();
 if (len > 0)
  { queue_encrypted(oq,&s->w.enc,data,s->w.until_rekey);
    s->w.until_rekey -= len;
  }
}

static void cflow_init(CFLOW *cf)
{
 cf->rj = rijndael_init(8,8);
 arc4_init(&cf->enc);
}

static void cstate_init(CSTATE *cs)
{
 cflow_init(&cs->r);
 cflow_init(&cs->w);
}

static int conn_destroy(void *cv)
{
 if (verbose) printf("conn_destroy destroying %p\n",cv);
 conn_free(cv);
 return(AIO_BLOCK_LOOP);
}

static void conn_abort(CONN *c)
{
 if (verbose) printf("conn_abort aborting %p\n",(void *)c);
 if (c->inlist)
  { if (c->flink) c->flink->blink = c->blink;
    if (c->blink) c->blink->flink = c->flink; else c->l->new = c->flink;
    c->inlist = 0;
    if (verbose) printf("unlinked\n");
  }
 if (c->l->client->live == c)
  { client_nolive(c->l->client);
    if (verbose) printf("removed live\n");
  }
 if (c->pid != AIO_NOID)
  { aio_remove_poll(c->pid);
    c->pid = AIO_NOID;
    if (verbose) printf("removed poll\n");
  }
 if (c->bid == AIO_NOID)
  { c->bid = aio_add_block(&conn_destroy,c);
    if (verbose) printf("set up destroy\n");
  }
}

static int conn_wtest(void *cv)
{
 return(aio_oq_nonempty(&((CONN *)cv)->oq));
}

static void conn_do_step1(CONN *c)
{
 if (verbose) printf("%p client nonce %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	(void *)c,
	c->cn[0],c->cn[1],c->cn[2],c->cn[3],c->cn[4],c->cn[5],c->cn[6],c->cn[7],
	c->cn[8],c->cn[9],c->cn[10],c->cn[11],c->cn[12],c->cn[13],c->cn[14],c->cn[15]);
 crypto_gen_mks(&c->cn[0],&c->sn[0],c->l->client->secret,c->l->client->secretlen,&c->cs.smk[0],&c->cs.cmk[0]);
 if (verbose)
  { printf("%p smk %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	(void *)c,
	c->cs.smk[0],c->cs.smk[1],c->cs.smk[2],c->cs.smk[3],c->cs.smk[4],c->cs.smk[5],c->cs.smk[6],c->cs.smk[7],
	c->cs.smk[8],c->cs.smk[9],c->cs.smk[10],c->cs.smk[11],c->cs.smk[12],c->cs.smk[13],c->cs.smk[14],c->cs.smk[15],
	c->cs.smk[16],c->cs.smk[17],c->cs.smk[18],c->cs.smk[19],c->cs.smk[20],c->cs.smk[21],c->cs.smk[22],c->cs.smk[23],
	c->cs.smk[24],c->cs.smk[25],c->cs.smk[26],c->cs.smk[27],c->cs.smk[28],c->cs.smk[29],c->cs.smk[30],c->cs.smk[31]);
    printf("%p cmk %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	(void *)c,
	c->cs.cmk[0],c->cs.cmk[1],c->cs.cmk[2],c->cs.cmk[3],c->cs.cmk[4],c->cs.cmk[5],c->cs.cmk[6],c->cs.cmk[7],
	c->cs.cmk[8],c->cs.cmk[9],c->cs.cmk[10],c->cs.cmk[11],c->cs.cmk[12],c->cs.cmk[13],c->cs.cmk[14],c->cs.cmk[15],
	c->cs.cmk[16],c->cs.cmk[17],c->cs.cmk[18],c->cs.cmk[19],c->cs.cmk[20],c->cs.cmk[21],c->cs.cmk[22],c->cs.cmk[23],
	c->cs.cmk[24],c->cs.cmk[25],c->cs.cmk[26],c->cs.cmk[27],c->cs.cmk[28],c->cs.cmk[29],c->cs.cmk[30],c->cs.cmk[31]);
  }
 crypto_rekey_w(&c->cs,&c->oq);
 c->cs.r.until_rekey = 0;
 crypto_nonce(&c->vsend[0],16);
 queue_data(&c->oq,&c->cs,&c->vsend[0],16);
 c->step = 2;
}

static void client_tap_rd(void *cv)
{
 CLIENT *c;
 unsigned char pkt[2000];
 int len;

 c = cv;
 len = read(c->tapfd,&pkt[2],sizeof(pkt)-2);
 if (len < 0)
  { switch (errno)
     { case EINTR:
       case EWOULDBLOCK:
	  return;
	  break;
     }
    fprintf(stderr,"%s: read from %s: %s\n",__progname,c->tapname,strerror(errno));
    exit(1);
  }
 if (verbose) printf("%p: got packet from %s, length %d: ",(void *)c,c->tapname,len);
 if (! c->live)
  { if (verbose) printf("no live conn\n");
    return;
  }
 if (aio_oq_qlen(&c->live->oq) > 65536)
  { if (verbose) printf("dropping, queue size %d\n",aio_oq_qlen(&c->live->oq));
    return;
  }
 if (verbose) printf("sending\n");
 pkt[0] = len >> 8;
 pkt[1] = len & 0xff;
 queue_data(&c->live->oq,&c->live->cs,&pkt[0],2+len);
}

static void ensure_interfaces(CLIENT *c)
{
 int fd;
 struct ifreq ifr;
 struct ifaliasreq ifar;

 if (c->tapfd >= 0)
  { if (verbose) printf("ensure_interfaces %s: already have\n",c->name);
    return;
  }
 fd = open(tap_path,O_RDWR,0);
 if (fd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,tap_path,strerror(errno));
    exit(1);
  }
 if (ioctl(fd,TAPGIFNAME,&ifr) < 0)
  { fprintf(stderr,"%s: can't TAPGIFNAME: %s\n",__progname,strerror(errno));
    exit(1);
  }
 c->tapname = strndup(&ifr.ifr_name[0],sizeof(ifr.ifr_name));
 if (verbose) printf("ensure_interfaces %s: using %s\n",c->name,c->tapname);
 // Lots of "XXX API botch" here....
 bzero(&ifar,sizeof(ifar));
 strncpy(&ifar.ifra_name[0],c->tapname,sizeof(ifar.ifra_name));
 ((struct sockaddr_in *)&ifar.ifra_addr)->sin_family = AF_INET;
 ((struct sockaddr_in *)&ifar.ifra_addr)->sin_len = sizeof(struct sockaddr_in);
 ((struct sockaddr_in *)&ifar.ifra_addr)->sin_addr = *c->baseip;
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_addr)->sin_addr)[3] |= 1;
 ((struct sockaddr_in *)&ifar.ifra_broadaddr)->sin_family = AF_INET;
 ((struct sockaddr_in *)&ifar.ifra_broadaddr)->sin_len = sizeof(struct sockaddr_in);
 ((struct sockaddr_in *)&ifar.ifra_broadaddr)->sin_addr = *c->baseip;
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_broadaddr)->sin_addr)[3] |= 3;
 ((struct sockaddr_in *)&ifar.ifra_mask)->sin_family = AF_INET;
 ((struct sockaddr_in *)&ifar.ifra_mask)->sin_len = sizeof(struct sockaddr_in);
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_mask)->sin_addr)[0] = 0xff;
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_mask)->sin_addr)[1] = 0xff;
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_mask)->sin_addr)[2] = 0xff;
 ((unsigned char *)&((struct sockaddr_in *)&ifar.ifra_mask)->sin_addr)[3] = 0xfc;
 if (ioctl(ifsock,SIOCAIFADDR,&ifar) < 0)
  { fprintf(stderr,"%s: can't SIOCAIFADDR %.*s: %s\n",__progname,(int)sizeof(ifar.ifra_name),&ifar.ifra_name[0],strerror(errno));
    exit(1);
  }
 bzero(&ifr,sizeof(ifr));
 strncpy(&ifr.ifr_name[0],c->tapname,sizeof(ifr.ifr_name));
 if (ioctl(ifsock,SIOCGIFFLAGS,&ifr) < 0)
  { fprintf(stderr,"%s: can't SIOCGIFFLAGS %.*s: %s\n",__progname,(int)sizeof(ifr.ifr_name),&ifr.ifr_name[0],strerror(errno));
    exit(1);
  }
 ifr.ifr_flags |= IFF_UP;
 if (ioctl(ifsock,SIOCSIFFLAGS,&ifr) < 0)
  { fprintf(stderr,"%s: can't SIOCSIFFLAGS %.*s UP: %s\n",__progname,(int)sizeof(ifr.ifr_name),&ifr.ifr_name[0],strerror(errno));
    exit(1);
  }
/*
 bzero(&ifbr,sizeof(ifbr));
 strncpy(&ifbr.ifbr_ifsname[0],c->tapname,sizeof(ifbr.ifbr_ifsname));
 bzero(&ifd,sizeof(ifd));
 strncpy(&ifd.ifd_name[0],c->bridgename,sizeof(ifd.ifd_name));
 ifd.ifd_cmd = BRDGADD;
 ifd.ifd_len = sizeof(ifbr);
 ifd.ifd_data = &ifbr;
 if (ioctl(ifsock,SIOCSDRVSPEC,&ifd) < 0)
  { fprintf(stderr,"%s: can't SIOCSDRVSPEC to add %.*s to %.*s: %s\n",
	__progname,
	(int)sizeof(ifbr.ifbr_ifsname), &ifbr.ifbr_ifsname[0],
	(int)sizeof(ifd.ifd_name), &ifd.ifd_name[0],
	strerror(errno));
    exit(1);
  }
*/
 c->tapfd = fd;
 c->tapid = aio_add_poll(c->tapfd,&aio_rwtest_always,&aio_rwtest_never,&client_tap_rd,0,c);
}

static void conn_success(CONN *c)
{
 if (c->l->client->live) conn_abort(c->l->client->live);
 c->l->client->live = c;
 ensure_interfaces(c->l->client);
}

static void conn_got_packet(CONN *c, const unsigned char *data, int len)
{
 if (verbose) printf("got packet from %s - %s, length %d\n",c->text,c->l->text,len);
 write(c->l->client->tapfd,data,len);
}

/*
 * We can get away without checking cs.r.until_rekey for cases 2 and 3
 *  because too little data flows to ever need a rekey there, once the
 *  first rekey is done.
 */
static int conn_input(CONN *c, const unsigned char *data, int len)
{
 int l;
 unsigned char *bp;
 int want;
 int pl;

 if (verbose) printf("conn_input %p step %d len %d\n",(void *)c,c->step,len);
 if (c->step == 1)
  { l = 16 - c->fill;
    if (l > len) l = len;
    bcopy(data,&c->cn[c->fill],l);
    c->fill += l;
    if (c->fill >= 16)
     { conn_do_step1(c);
       c->fill = 0;
     }
    if (verbose) printf("step1 returning %d\n",l);
    return(l);
  }
 if (c->cs.r.until_rekey < 1)
  { l = 33 - c->fill;
    if (l > len) l = len;
    bcopy(data,&c->cs.r.ke[c->fill],l);
    c->fill += l;
    if (c->fill >= 33)
     { crypto_rekey_r(&c->cs);
       c->fill = 0;
     }
    if (verbose) printf("need rekey returning %d\n",l);
    return(l);
  }
 l = c->cs.r.until_rekey;
 if (l > len) l = len;
 switch (c->step)
  { case 2:
       bp = &c->cv[0];
       want = 16;
       break;
    case 3:
       bp = &c->vrecv[0];
       want = 16;
       break;
    case 0:
       bp = &c->packet[0];
       want = (c->fill < 2) ? 2 : (2 + (c->packet[0] * 256) + c->packet[1]);
       break;
    default:
       abort();
       break;
  }
 if (l > want - c->fill) l = want - c->fill;
 if (l < 0) abort();
 arc4_crypt(&c->cs.r.enc,data,l,bp+c->fill);
 if (verbose)
  { printf("decrypted %d:\n",l);
    dump_data(stdout,bp+c->fill,l);
  }
 c->fill += l;
 if (c->fill >= want)
  { c->fill = 0;
    switch (c->step)
     { case 2:
	  queue_data(&c->oq,&c->cs,&c->cv[0],16);
	  c->step = 3;
	  break;
       case 3:
	  if (bcmp(&c->vsend[0],&c->vrecv[0],16))
	   { if (verbose) printf("verifier failed\n");
	     conn_abort(c);
	     return(len);
	   }
	  conn_success(c);
	  c->step = 0;
	  break;
       case 0:
	  pl = (c->packet[0] * 256) + c->packet[1];
	  if (want == 2+pl)
	   { conn_got_packet(c,&c->packet[2],pl);
	   }
	  else if (want != 2)
	   { printf("impossible packet want\n");
	     abort();
	   }
	  else
	   { c->fill = 2;
	   }
	  break;
     }
  }
 if (verbose) printf("normal return %d\n",l);
 return(l);
}

static void conn_rd(void *cv)
{
 CONN *c;
 int r;
 unsigned char rbuf[512];
 int o;
 int p;

 c = cv;
 r = read(c->fd,&rbuf[0],sizeof(rbuf));
 if (r < 0)
  { switch (errno)
     { case EINTR:
       case EWOULDBLOCK:
	  return;
	  break;
     }
    fprintf(stderr,"%s: read error from %s: %s\n",__progname,c->text,strerror(errno));
    conn_abort(c);
    return;
  }
 if (verbose)
  { printf("%p read %d:\n",(void *)c,r);
    dump_data(stdout,&rbuf[0],r);
  }
 if (r == 0)
  { conn_abort(c);
    return;
  }
 o = 0;
 while (1)
  { p = conn_input(c,&rbuf[o],r-o);
    o += p;
    if (o > r) abort();
    if (o == r) break;
  }
}

static void conn_wr(void *cv)
{
 CONN *c;
 int w;

 c = cv;
 w = aio_oq_writev(&c->oq,c->fd,-1);
 if (w < 0)
  { switch (errno)
     { case EINTR:
       case EWOULDBLOCK:
	  return;
	  break;
     }
    fprintf(stderr,"%s: write error to %s: %s\n",__progname,c->text,strerror(errno));
    conn_abort(c);
    return;
  }
 aio_oq_dropdata(&c->oq,w);
}

static void set_nonblock(int fd)
{
 fcntl(fd,F_SETFL,fcntl(fd,F_GETFL,0)|O_NONBLOCK);
}

static void listener_accept(void *lv)
{
 LISTEN *l;
 int s;
 struct sockaddr_storage ss;
 socklen_t sslen;
 CONN *c;
 char gnihost[NI_MAXHOST];
 char gniserv[NI_MAXSERV];
 int e;
 char *t;

 l = lv;
 sslen = sizeof(ss);
 s = accept(l->afd,(void *)&ss,&sslen);
 if (s < 0)
  { if (errno == EWOULDBLOCK) return;
    fprintf(stderr,"%s: accept [%s]: %s\n",__progname,l->text,strerror(errno));
    return;
  }
 fcntl(s,F_SETFD,1);
 set_nonblock(s);
 e = getnameinfo((void *)&ss,sslen,&gnihost[0],NI_MAXHOST,&gniserv[0],NI_MAXSERV,NI_NUMERICHOST|NI_NUMERICSERV);
 if (e)
  { asprintf(&t,"[getnameinfo err: %s]",gai_strerror(e));
  }
 else
  { asprintf(&t,"%s/%s",&gnihost[0],&gniserv[0]);
  }
 c = malloc(sizeof(CONN));
 c->text = t;
 c->fd = s;
 c->pid = aio_add_poll(s,&aio_rwtest_always,&conn_wtest,&conn_rd,&conn_wr,c);
 c->bid = AIO_NOID;
 c->l = l;
 aio_oq_init(&c->oq);
 c->inlist = 1;
 c->flink = l->new;
 c->blink = 0;
 if (c->flink) c->flink->blink = c;
 l->new = c;
 c->step = 1;
 c->fill = 0;
 cstate_init(&c->cs);
 crypto_nonce(&c->sn[0],16);
 aio_oq_queue_point(&c->oq,&c->sn,16);
 if (verbose)
  { printf("accepted %p from %s\n",(void *)c,t);
    printf("%p server nonce %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
	(void *)c,
	c->sn[0],c->sn[1],c->sn[2],c->sn[3],c->sn[4],c->sn[5],c->sn[6],c->sn[7],
	c->sn[8],c->sn[9],c->sn[10],c->sn[11],c->sn[12],c->sn[13],c->sn[14],c->sn[15]);
  }
}

static int setup_listener(LISTEN *l)
{
 int s;

 s = socket(AF_INET,SOCK_STREAM,0);
 if (s < 0)
  { fprintf(stderr,"%s: listener [%s] socket: %s\n",__progname,l->text,strerror(errno));
    return(-1);
  }
 if (bind(s,l->sa,l->salen) < 0)
  { fprintf(stderr,"%s: listener [%s] bind: %s\n",__progname,l->text,strerror(errno));
    close(s);
    return(-1);
  }
 if (listen(s,10) < 0)
  { fprintf(stderr,"%s: listener [%s] listen: %s\n",__progname,l->text,strerror(errno));
    close(s);
    return(-1);
  }
 fcntl(s,F_SETFD,1);
 set_nonblock(s);
 l->afd = s;
 l->aid = aio_add_poll(s,&aio_rwtest_always,&aio_rwtest_never,&listener_accept,0,l);
 return(0);
}

static int setup_listeners(CONFIG *cf)
{
 CLIENT *cl;
 LISTEN *l;
 int rv;

 rv = 0;
 for (cl=cf->clients;cl;cl=cl->link)
  { for (l=cl->listen;l;l=l->link)
     { if (setup_listener(l) < 0) rv = -1;
     }
  }
 return(rv);
}

static void setup_devnull(void)
{
 devnull = open(devnull_path,O_RDWR,0);
 if (devnull < 0)
  { fprintf(stderr,"%s: open %s: %s\n",__progname,devnull_path,strerror(errno));
    exit(1);
  }
 while (devnull < 3)
  { devnull = dup(devnull);
    if (devnull < 0)
     { fprintf(stderr,"%s: dup on %s: %s\n",__progname,devnull_path,strerror(errno));
       exit(1);
     }
  }
}

static void setup_ifsock(void)
{
 // XXX AF_LOCAL should work here, but doesn't.
 // XXX Should figure out why and fix it.
 ifsock = socket(AF_INET,SOCK_DGRAM,0);
 if (ifsock < 0)
  { fprintf(stderr,"%s: socket(AF_LOCAL,SOCK_DGRAM,0): %s\n",__progname,strerror(errno));
    exit(1);
  }
}

static void randtweak_rd(void *arg __attribute__((__unused__)))
{
 struct timersock_event tse[64];
 int nr;

 nr = read(randtweak_fd,&tse[0],sizeof(tse));
 if (nr < 0)
  { switch (errno)
     { case EINTR:
       case EWOULDBLOCK:
	  return;
	  break;
     }
    fprintf(stderr,"%s: timer socket read: %s\n",__progname,strerror(errno));
    exit(1);
  }
 if (nr == 0)
  { fprintf(stderr,"%s: timer socket read EOF\n",__progname);
    exit(1);
  }
 randtweak = 1;
}

static void setup_random(void)
{
 struct itimerval itv;
 struct timeval now;
 int i;
 unsigned char rdat[4];

 randfd = open(random_path,O_RDONLY,0);
 if (randfd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,random_path,strerror(errno));
    exit(1);
  }
 randtweak_fd = socket(AF_TIMER,SOCK_STREAM,0);
 if (randtweak_fd < 0)
  { fprintf(stderr,"%s: can't create timer socket: %s\n",__progname,strerror(errno));
    exit(1);
  }
 set_nonblock(randtweak_fd);
 itv.it_interval.tv_sec = 60;
 itv.it_interval.tv_usec = 123456;
 itv.it_value = itv.it_interval;
 write(randtweak_fd,&itv,sizeof(itv));
 randid = aio_add_poll(randtweak_fd,&aio_rwtest_always,&aio_rwtest_never,&randtweak_rd,0,0);
 randtweak = 0;
 gettimeofday(&now,0);
 i = sizeof(now);
 if (i > sizeof(randpool)) i = sizeof(randpool);
 for (i--;i>=0;i--) ((unsigned char *)&randpool)[i] ^= ((unsigned char *)&now)[i];
 read(randfd,&rdat[0],4);
 ((unsigned char *)&randpool)[sizeof(randpool)-1] ^= rdat[0];
 ((unsigned char *)&randpool)[sizeof(randpool)-2] ^= rdat[1];
 ((unsigned char *)&randpool)[sizeof(randpool)-3] ^= rdat[2];
 ((unsigned char *)&randpool)[sizeof(randpool)-4] ^= rdat[3];
 random_stir();
 randptr = 0;
}

static void catch_sig(int sig)
{
 char c;

 switch (sig)
  { case SIGINFO: c = 'i'; break;
    case SIGUSR1: c = '1'; break;
    case SIGUSR2: c = '2'; break;
    default: return; break;
  }
 write(verbpipe[1],&c,1);
}

static void process_signal_flag(char c)
{
 switch (c)
  { case 'i':
       fprintf(stderr,"SIGINFO output\n");
       break;
    case '1':
       verbose ++;
       fprintf(stderr,"SIGUSR1: verbose now %d\n",verbose);
       break;
    case '2':
       verbose --;
       if (verbose < 0) verbose = 0;
       fprintf(stderr,"SIGUSR2: verbose now %d\n",verbose);
       break;
    default:
       abort();
       break;
  }
}

static void verbpipe_rd(void *arg __attribute__((__unused__)))
{
 char buf[64];
 int nr;
 int i;

 nr = read(verbpipe[0],&buf[0],sizeof(buf));
 if (nr < 0)
  { switch (errno)
     { case EINTR:
       case EWOULDBLOCK:
	  return;
	  break;
     }
    fprintf(stderr,"%s: signal pipe read: %s\n",__progname,strerror(errno));
    exit(1);
  }
 for (i=0;i<nr;i++) process_signal_flag(buf[i]);
}

static void setup_verbose(void)
{
 struct sigaction sa;

 verbose = 0;
 if (pipe(&verbpipe[0]) < 0)
  { fprintf(stderr,"%s: pipe: %s\n",__progname,strerror(errno));
    exit(1);
  }
 set_nonblock(verbpipe[0]);
 set_nonblock(verbpipe[1]);
 aio_add_poll(verbpipe[0],&aio_rwtest_always,&aio_rwtest_never,&verbpipe_rd,0,0);
 sa.sa_sigaction = 0;
 sa.sa_handler = &catch_sig;
 sigemptyset(&sa.sa_mask);
 sa.sa_flags = SA_RESTART;
 sigaction(SIGINFO,&sa,0);
 sigaction(SIGUSR1,&sa,0);
 sigaction(SIGUSR2,&sa,0);
}

int main(int, char **);
int main(int ac, char **av)
{
 aio_poll_init();
 setup_verbose();
 setup_devnull();
 setup_random();
 setup_ifsock();
 handleargs(ac,av);
 conf = config_load();
 if (! conf) return(0);
 if (setup_listeners(conf) < 0) return(0);
 aio_event_loop();
 exit(1);
}