/*
 * Daemon to serve download requests from dl.s.
 *
 * Usage: $0 [flags] interface file
 * eg, dldaemon rtk0 kernel.bin
 *
 * flags can be:
 *
 *	-bpf PATH
 *		Use this instead of /dev/bpf.
 *
 *	-if INTF
 *		Like first arg, but unambiguous.
 *
 *	-file PATH
 *		Like second arg, but unambiguous.
 */

#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/mman.h>
#include <sys/ioctl.h>

extern const char *__progname;

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

static int bpffd;
static int filefd;
static struct stat filestat;
static unsigned char *filemap;

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

static int map_file(int fd, const struct stat *stb, unsigned char **ptr)
{
 void *mmrv;
 size_t sz;

 sz = stb->st_size;
 if (sz != stb->st_size)
  { errno = EFBIG;
    return(-1);
  }
 mmrv = mmap(0,sz,PROT_READ,MAP_FILE|MAP_PRIVATE,fd,0);
 if (mmrv == MAP_FAILED) return(-1);
 *ptr = mmrv;
 return(0);
}

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: build 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],arg1,sizeof(ifr.ifr_name));
 if (ioctl(bpffd,BIOCSETIF,&ifr) < 0)
  { fprintf(stderr,"%s: can't set interface %s: %s\n",__progname,arg1,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);
 filefd = open(arg2,O_RDONLY,0);
 if (filefd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,arg2,strerror(errno));
    exit(1);
  }
 if (fstat(filefd,&filestat) < 0)
  { fprintf(stderr,"%s: can't fstat %s: %s\n",__progname,arg2,strerror(errno));
    exit(1);
  }
 if (map_file(filefd,&filestat,&filemap) < 0)
  { fprintf(stderr,"%s: can't mmap %s: %s\n",__progname,arg2,strerror(errno));
    exit(1);
  }
}

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 void ipkt_0x00(const unsigned char *ip, int pl __attribute__((__unused__)))
{
 printf("Starting serving %02x:%02x:%02x:%02x:%02x:%02x\n",ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
 bcopy(ip+6,&opkt[0],6);
 opkt[12] = ip[12];
 opkt[13] = ip[13];
 opkt[14] = 0x01;
 sendopkt(15);
}

static void freshen_filestat(void)
{
 int fd;
 struct stat stb;
 unsigned char *data;

 fd = open(arg2,O_RDONLY,0);
 if (fd < 0)
  { fprintf(stderr,"%s: can't open %s (%s), using cached data\n",
	__progname,arg2,strerror(errno));
    return;
  }
 if (fstat(fd,&stb) < 0)
  { fprintf(stderr,"%s: can't fstat %s (%s), using cached data\n",
	__progname,arg2,strerror(errno));
    close(fd);
    return;
  }
 if ( (stb.st_dev == filestat.st_dev) &&
      (stb.st_ino == filestat.st_ino) &&
      (stb.st_size == filestat.st_size) &&
      (stb.st_mtime == filestat.st_mtime) &&
      (stb.st_mtimensec == filestat.st_mtimensec) &&
      (stb.st_ctime == filestat.st_ctime) &&
      (stb.st_ctimensec == filestat.st_ctimensec) )
  { close(fd);
    return;
  }
 if (map_file(fd,&stb,&data) < 0)
  { fprintf(stderr,"%s: can't mmap %s (%s), using cached data\n",
	__progname,arg2,strerror(errno));
    close(fd);
    return;
  }
 close(filefd);
 munmap(filemap,filestat.st_size);
 filefd = fd;
 filestat = stb;
 filemap = data;
}

static void ipkt_0x02(const unsigned char *ip, int pl)
{
 unsigned int offset;
 unsigned int len;

 if (pl < 6+6+2+1+4+2)
  { printf("0x02 packet too short, ignoring\n");
    return;
  }
 offset = (ip[15] * 0x01000000) +
	  (ip[16] * 0x00010000) +
	  (ip[17] * 0x00000100) +
	  (ip[18] * 0x00000001);
 len = (ip[19] * 0x0100) +
       (ip[20] * 0x0001);
 if (len > 1024)
  { printf("0x02 packet has bad length %u, ignoring\n",len);
    return;
  }
 freshen_filestat();
 bcopy(ip+6,&opkt[0],6);
 opkt[12] = ip[12];
 opkt[13] = ip[13];
 if (offset < filestat.st_size)
  { int nb;
    nb = (filestat.st_size-offset < len) ? filestat.st_size-offset : len;
    opkt[14] = 0x03;
    bcopy(&ip[15],&opkt[15],4);
    opkt[19] = 0x00;
    opkt[20] = nb >> 8;
    opkt[21] = nb & 0xff;
    bcopy(filemap+offset,&opkt[22],nb);
    sendopkt(22+nb);
  }
 else if (offset == filestat.st_size)
  { printf("Sending EOF marker at %u to %02x:%02x:%02x:%02x:%02x:%02x\n",offset,ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
    opkt[14] = 0x03;
    bcopy(&ip[15],&opkt[15],4);
    opkt[19] = 0x00;
    opkt[20] = 0;
    opkt[21] = 0;
    sendopkt(22);
  }
 else
  { printf("Sending past-EOF marker at %u to %02x:%02x:%02x:%02x:%02x:%02x\n",offset,ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
    opkt[14] = 0x03;
    bcopy(&ip[15],&opkt[15],4);
    opkt[19] = 0x01;
    sendopkt(20);
  }
}

static void run(void)
{
 struct pollfd pfd;
 struct bpf_hdr bh;
 int ipo;
 int psz;
 int pdo;

 while <"main"> (1)
  { pfd.fd = bpffd;
    pfd.events = POLLIN | POLLRDNORM | POLLRDBAND;
    if (poll(&pfd,1,INFTIM) < 0)
     { if (errno == EINTR) continue;
       fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno));
       exit(1);
     }
    while (1)
     { ipktlen = read(bpffd,ipkt,ipktbuflen);
       if (ipktlen < 0)
	{ switch (errno)
	   { case EINTR:
	     case EWOULDBLOCK:
		continue <"main">;
		break;
	   }
	  fprintf(stderr,"%s: BPF read: %s\n",__progname,strerror(errno));
	  exit(1);
	}
       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;
	     if (bh.bh_caplen < 15)
	      { printf("too short\n");
	      }
	     else
	      { switch (ipkt[pdo+14])
		 { default:
		      printf("unrecognized opcode %02x\n",ipkt[pdo+14]);
		      break;
		   case 0x00:
		      ipkt_0x00(&ipkt[pdo],bh.bh_caplen);
		      break;
		   case 0x02:
		      ipkt_0x02(&ipkt[pdo],bh.bh_caplen);
		      break;
		 }
	      }
	   }
	  if (ipo+psz > ipktlen) ipo = ipktlen; else ipo += psz;
	}
     }
  }
}

int main(int, char **);
int main(int ac, char **av)
{
 handleargs(ac,av);
 setup();
 run();
 exit(0);
}