/*
 * Watch controller/keyboard status from rem.s.
 *
 * Usage: $0 [flags] interface
 * eg, dldaemon rtk0
 *
 * flags can be:
 *
 *	-bpf PATH
 *		Use this instead of /dev/bpf.
 *
 *	-if INTF
 *		Like first arg, but unambiguous.
 */

#include <aio.h>
#include <poll.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <net/if.h>
#include <strings.h>
#include <net/bpf.h>
#include <sys/ioctl.h>
#include <sys/timersock.h>

extern const char *__progname;

static const char *bpfdev = "/dev/bpf";
static const char *intfname = 0;

typedef unsigned long long int TIME;

static int bpffd;
static int bpfid;
/*
 * This really should be consted, but struct bpf_program doesn't const
 *  bf_insns (WTF not?!).
 */
static struct bpf_insn program[]
 = { BPF_STMT(BPF_LD+BPF_H+BPF_ABS,12),
     BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K,0xFEDB,0,2),
     BPF_STMT(BPF_LD+BPF_W+BPF_LEN,0),
     BPF_STMT(BPF_RET+BPF_A,0),
     BPF_STMT(BPF_RET+BPF_K,0) };
#define PROGRAM_LENGTH (sizeof(program) / sizeof(program[0]))

static unsigned char *ipkt;
static int ipktbuflen;
static int ipktlen;
static unsigned char opkt[2000];
static unsigned char server_mac[6];
static int have_server_mac;
static TIME pkttime;
static int want_hail;
static int hailid;
static int hailtimerfd;
static int hailtimerid;
static TIME lastpkt;
static int want_request;
static int reqid;
static int reqtimerfd;
static int reqtimerid;

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 != '-')
     { if (! intfname)
	{ intfname = *av;
	}
       else
	{ fprintf(stderr,"%s: extra 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,"-bpf"))
     { WANTARG();
       bpfdev = av[skip];
       continue;
     }
    if (!strcmp(*av,"-if"))
     { WANTARG();
       intfname = av[skip];
       continue;
     }
#undef WANTARG
    fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av);
    errs = 1;
  }
 if (! intfname)
  { fprintf(stderr,"%s: need an interface\n",__progname);
    errs = 1;
  }
 if (errs) exit(1);
}

static TIME getnow(void)
{
 struct timeval tv;

 gettimeofday(&tv,0);
 return(tv.tv_usec+(tv.tv_sec*(TIME)1000000));
}

static void setup(void)
{
 struct ifreq ifr;
 unsigned int ui;
 struct bpf_version ver;
 struct bpf_program pgm;

 bpffd = open(bpfdev,O_RDWR,0);
 if (bpffd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,bpfdev,strerror(errno));
    exit(1);
  }
 if (ioctl(bpffd,BIOCVERSION,&ver) < 0)
  { fprintf(stderr,"%s: can't get BPF version: %s\n",__progname,strerror(errno));
    exit(1);
  }
 if ( (ver.bv_major != BPF_MAJOR_VERSION) ||
      (ver.bv_minor < BPF_MINOR_VERSION) )
  { fprintf(stderr,"%s: BPF version mismatch: built for %d.%d, kernel has %d.%d\n",
	__progname,
	BPF_MAJOR_VERSION, BPF_MINOR_VERSION,
	ver.bv_major, ver.bv_minor );
    exit(1);
  }
 ui = 2000;
 if (ioctl(bpffd,BIOCSBLEN,&ui) < 0)
  { fprintf(stderr,"%s: can't set buffer size: %s\n",__progname,strerror(errno));
    exit(1);
  }
 if (ioctl(bpffd,BIOCGBLEN,&ui) < 0)
  { fprintf(stderr,"%s: can't get buffer size: %s\n",__progname,strerror(errno));
    exit(1);
  }
 ipktbuflen = ui;
 strncpy(&ifr.ifr_name[0],intfname,sizeof(ifr.ifr_name));
 if (ioctl(bpffd,BIOCSETIF,&ifr) < 0)
  { fprintf(stderr,"%s: can't set interface %s: %s\n",__progname,intfname,strerror(errno));
    exit(1);
  }
 if (ioctl(bpffd,BIOCGDLT,&ui) < 0)
  { fprintf(stderr,"%s: can't get data link type: %s\n",__progname,strerror(errno));
    exit(1);
  }
 if (ui != DLT_EN10MB)
  { fprintf(stderr,"%s: data link type is %d, wanted %d\n",__progname,ui,DLT_EN10MB);
    exit(1);
  }
 ui = 1;
 if (ioctl(bpffd,BIOCIMMEDIATE,&ui) < 0)
  { fprintf(stderr,"%s: can't set immediate mode: %s\n",__progname,strerror(errno));
    exit(1);
  }
 ui = 0;
 if (ioctl(bpffd,BIOCSHDRCMPLT,&ui) < 0)
  { fprintf(stderr,"%s: can't set no-header-complete mode: %s\n",__progname,strerror(errno));
    exit(1);
  }
 ui = 0;
 if (ioctl(bpffd,BIOCSSEESENT,&ui) < 0)
  { fprintf(stderr,"%s: can't set no-see-sent mode: %s\n",__progname,strerror(errno));
    exit(1);
  }
 ipkt = malloc(ipktbuflen);
 pgm.bf_len = PROGRAM_LENGTH;
 pgm.bf_insns = &program[0];
 if (ioctl(bpffd,BIOCSETF,&pgm) < 0)
  { fprintf(stderr,"%s: can't set filter: %s\n",__progname,strerror(errno));
    exit(1);
  }
 fcntl(bpffd,F_SETFL,fcntl(bpffd,F_GETFL,0)|O_NONBLOCK);
}

static void sendopkt(int len)
{
 int w;

 w = write(bpffd,&opkt[0],len);
 if (w < 0)
  { fprintf(stderr,"%s: BPF write: %s\n",__progname,strerror(errno));
  }
 else if (w != len)
  { fprintf(stderr,"%s: BPF write: tried %d, wrote %d\n",__progname,len,w);
  }
}

static int send_hail(void *arg __attribute__((__unused__)))
{
 if (! want_hail) return(AIO_BLOCK_NIL);
 want_hail = 0;
 memset(&opkt[0],0xff,6);
 opkt[12] = 0xfe;
 opkt[13] = 0xdb;
 opkt[14] = 0x00;
 sendopkt(15);
 lastpkt = getnow();
 return(AIO_BLOCK_LOOP);
}

static int send_request(void *arg __attribute__((__unused__)))
{
 if (! want_request) return(AIO_BLOCK_NIL);
 want_request = 0;
 if (! have_server_mac) return(AIO_BLOCK_NIL);
 bcopy(&server_mac[0],&opkt[0],6);
 opkt[12] = 0xfe;
 opkt[13] = 0xdb;
 opkt[14] = 0x02;
 sendopkt(15);
 lastpkt = getnow();
 return(AIO_BLOCK_LOOP);
}

static void hail_timer_rd(void *arg __attribute__((__unused__)))
{
 struct timersock_event tse;
 int rrv;
 int want;

 want = 0;
 while (1)
  { rrv = read(hailtimerfd,&tse,sizeof(tse));
    if (rrv == sizeof(tse))
     { want = 1;
     }
    else if (rrv >= 0)
     { fprintf(stderr,"%s: strange timersock read %d\n",__progname,rrv);
       exit(1);
     }
    else
     { switch (errno)
	{ case EINTR:
	  case EWOULDBLOCK:
	     if (want && (getnow() > lastpkt+1000000))
	      { have_server_mac = 0;
		want_hail = 1;
	      }
	     return;
	}
       fprintf(stderr,"%s: timersock read: %s\n",__progname,strerror(errno));
       exit(1);
     }
  }
}

static void req_timer_rd(void *arg __attribute__((__unused__)))
{
 struct timersock_event tse;
 int rrv;
 int any;

 any = 0;
 while (1)
  { rrv = read(reqtimerfd,&tse,sizeof(tse));
    if (rrv == sizeof(tse))
     { any = 1;
     }
    else if (rrv >= 0)
     { fprintf(stderr,"%s: strange timersock read %d\n",__progname,rrv);
       exit(1);
     }
    else
     { switch (errno)
	{ case EINTR:
	  case EWOULDBLOCK:
	     if (any && have_server_mac) want_request = 1;
	     return;
	}
       fprintf(stderr,"%s: timersock read: %s\n",__progname,strerror(errno));
       exit(1);
     }
  }
}

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

static void start_hail_timer(void)
{
 struct itimerval itv;

 hailtimerfd = socket(AF_TIMER,SOCK_STREAM,0);
 setnbio(hailtimerfd);
 hailtimerid = aio_add_poll(hailtimerfd,&aio_rwtest_always,&aio_rwtest_never,&hail_timer_rd,0,0);
 itv.it_value.tv_sec = 1;
 itv.it_value.tv_usec = 0;
 itv.it_interval = itv.it_value;
 write(hailtimerfd,&itv,sizeof(itv));
}

static void start_req_timer(void)
{
 struct itimerval itv;

 reqtimerfd = socket(AF_TIMER,SOCK_STREAM,0);
 setnbio(reqtimerfd);
 reqtimerid = aio_add_poll(reqtimerfd,&aio_rwtest_always,&aio_rwtest_never,&req_timer_rd,0,0);
 itv.it_value.tv_sec = 0;
 itv.it_value.tv_usec = 25000;
 itv.it_interval = itv.it_value;
 write(reqtimerfd,&itv,sizeof(itv));
}

static void dump_response(const unsigned char *dp)
{
 int i;

 printf("cycles=%02x%02x%02x%02x%02x%02x%02x%02x",
	dp[7], dp[6], dp[5], dp[4], dp[3], dp[2], dp[1], dp[0]);
 dp += 8;
 for (i=1;i<=4;i++)
  { printf(" %d:",i);
    switch (*dp++)
     { case 0x00:
	  printf("-");
	  break;
       case 0x01:
	  printf("?");
	  break;
       case 0x02:
	  printf("C=%02x%02x%02x%02x%02x%02x%02x%02x",
		dp[0], dp[1], dp[2], dp[3],
		dp[4], dp[5], dp[6], dp[7] );
	  dp += 8;
	  break;
       case 0x03:
	  printf("K=%02x%02x%02x%02x%02x%02x%02x%02x",
		dp[0], dp[1], dp[2], dp[3],
		dp[4], dp[5], dp[6], dp[7] );
	  dp += 8;
	  break;
       default:
	  printf("BAD(%02x)",dp[-1]);
	  break;
     }
  }
 printf("\r");
 fflush(stdout);
}

static void got_packet(const unsigned char *data, int len)
{
 if (len < 15)
  { printf("packet too short (len = %d)\n",len);
    return;
  }
 switch (data[14])
  { case 0x01: /* hail ack */
       lastpkt = pkttime;
       bcopy(&data[6],&server_mac[0],6);
       have_server_mac = 1;
       printf("Server MAC %02x:%02x:%02x:%02x:%02x:%02x\n",
		server_mac[0], server_mac[1], server_mac[2],
		server_mac[3], server_mac[4], server_mac[5] );
       break;
    case 0x03: /* response */
       dump_response(&data[15]);
       break;
    default:
       printf("unrecognized opcode %02x\n",data[14]);
       break;
  }
}

static void bpf_rd(void *arg __attribute__((__unused__)))
{
 int ipo;
 struct bpf_hdr bh;
 int psz;
 int pdo;

 while (1)
  { ipktlen = read(bpffd,ipkt,ipktbuflen);
    if (ipktlen < 0)
     { switch (errno)
	{ case EINTR:
	     continue;
	     break;
	  case EWOULDBLOCK:
	     return;
	     break;
	}
       fprintf(stderr,"%s: BPF read: %s\n",__progname,strerror(errno));
       exit(1);
     }
    pkttime = getnow();
    ipo = 0;
    while (1)
     { if (ipo > ipktlen) abort();
       if (ipo == ipktlen) break;
       if (ipktlen-ipo < sizeof(struct bpf_hdr))
	{ fprintf(stderr,"%s: BPF error: bytes left (%d) < header size (%d)\n",
	     __progname, ipktlen-ipo, (int)sizeof(struct bpf_hdr) );
	  break;
	}
       bcopy(ipkt+ipo,&bh,sizeof(struct bpf_hdr));
       if (ipktlen-ipo < bh.bh_hdrlen+bh.bh_caplen)
	{ fprintf(stderr,"%s: BPF error: bytes left (%d) < packet size (%d)\n",
	     __progname, ipktlen-ipo, (int)(bh.bh_hdrlen+bh.bh_caplen) );
	  break;
	}
       psz = BPF_WORDALIGN(bh.bh_hdrlen+bh.bh_caplen);
       if (bh.bh_caplen < bh.bh_datalen)
	{ fprintf(stderr,"%s: BPF error: packet got truncated (caplen %d != datalen %d)\n",
	  __progname, (int)bh.bh_caplen, (int)bh.bh_datalen );
	}
       else
	{ pdo = ipo + bh.bh_hdrlen;
	  got_packet(ipkt+pdo,bh.bh_caplen);
	}
       if (ipo+psz > ipktlen) ipo = ipktlen; else ipo += psz;
     }
  }
}

int main(int, char **);
int main(int ac, char **av)
{
 handleargs(ac,av);
 setup();
 aio_poll_init();
 have_server_mac = 0;
 bpfid = aio_add_poll(bpffd,&aio_rwtest_always,&aio_rwtest_never,&bpf_rd,0,0);
 want_hail = 1;
 lastpkt = 0;
 hailid = aio_add_block(&send_hail,0);
 start_hail_timer();
 want_request = 0;
 reqid = aio_add_block(&send_request,0);
 start_req_timer();
 while (1)
  { aio_pre_poll();
    if (aio_do_poll() < 0)
     { if (errno == EINTR) continue;
       fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno));
       exit(1);
     }
    aio_post_poll();
  }
 exit(0);
}