/*
 * 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;

typedef enum {
	  FT_UNSET = 1,
	  FT_GUESS,
	  FT_BIN,
	  FT_SREC
	  } FILETYPE;

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

typedef struct fileseg FILESEG;
typedef struct filedata FILEDATA;
#define FILEDATA_CHUNK 65500

struct fileseg {
  unsigned int base;
  unsigned int len;
  unsigned char *data;
  } ;

struct filedata {
  FILEDATA *link;
  int fill;
  /* data follows */
  } ;

static int bpffd;
static int filefd;
static struct stat filestat;
static unsigned char *filemap;
static unsigned int entry;
static int haveentry;
static FILESEG *filesegs;
static int nfilesegs;
static FILEDATA *filedata;
static unsigned int maxseglen;
static int cookiebase;

/*
 * 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;
	  arg2type = FT_GUESS;
	}
       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];
       arg2type = FT_GUESS;
       continue;
     }
    if (!strcmp(*av,"-bin"))
     { WANTARG();
       arg2 = av[skip];
       arg2type = FT_BIN;
       continue;
     }
    if (!strcmp(*av,"-srec"))
     { WANTARG();
       arg2 = av[skip];
       arg2type = FT_SREC;
       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;
 int sz;

 sz = stb->st_size;
 if ((sz < 0) || (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 int parse_srec(
	const unsigned char *data,
	int len,
	int (*rec)(int, int, const unsigned char *),
	void (*err)(const char *) )
{
 int i;
 int i0;
 int j;
 int n;
 int dv;
 int bv;
 int rt;
 int rv;
 const unsigned char *r;
 unsigned char bytes[50];
 int nock;
 unsigned int ck;

 rv = 0;
 i0 = -1;
 for (i=0;i<len;i++)
  { switch (filemap[i])
     { case '\r': case '\n':
	  if (i0 < 0) break;
	  n = i - i0;
	  if (n < 14) (*err)("record too short");
	  r = data + i0;
	  if (r[0] != 'S')
	   { (*err)("record doesn't begin with S");
	     return(-1);
	   }
	  switch (r[1])
	   { case '3': rt = 3; break;
	     case '7': rt = 7; break;
	     default:
		(*err)("bad record type");
		return(-1);
		break;
	   }
	  nock = 0;
	  ck = 0;
	  for (j=2;j<n;j++)
	   { switch <"digit"> (r[j])
	      { case '0': dv =  0; break;
		case '1': dv =  1; break;
		case '2': dv =  2; break;
		case '3': dv =  3; break;
		case '4': dv =  4; break;
		case '5': dv =  5; break;
		case '6': dv =  6; break;
		case '7': dv =  7; break;
		case '8': dv =  8; break;
		case '9': dv =  9; break;
		case 'A': dv = 10; break;
		case 'B': dv = 11; break;
		case 'C': dv = 12; break;
		case 'D': dv = 13; break;
		case 'E': dv = 14; break;
		case 'F': dv = 15; break;
		case '?':
		   switch (n-j)
		    { case 1: case 2:
			 dv = 0;
			 nock |= n - j;
			 break;
		      default:
		default <"digit">:
			 (*err)("record contains invalid character");
			 return(-1);
			 break;
		    }
		   break;
	      }
	     if (j & 1)
	      { bv = (bv << 4) | dv;
		bytes[(j-2)>>1] = bv;
		ck += bv;
	      }
	     else
	      { bv = dv;
	      }
	   }
	  if (n != 4+(bytes[0]*2)) (*err)("record length wrong");
	  switch (nock)
	   { case 1: case 2:
		(*err)("record contains invalid character");
		return(-1);
		break;
	     case 0:
		if ((ck & 0xff) != 0xff)
		 { (*err)("record checksum wrong");
		   return(-1);
		   break;
		 }
		j = (*rec)(rt,bytes[0]-1,&bytes[1]);
		if (j < 0) return(j);
		rv += j;
		break;
	     case 3:
		break;
	   }
	  i0 = -1;
	  break;
       default:
	  if (i0 < 0) i0 = i;
	  if (i-i0 > 100)
	   { (*err)("record too long");
	     return(-1);
	   }
	  break;
     }
  }
 if (i0 >= 0)
  { (*err)("unterminated last record");
    return(-1);
  }
 return(rv);
}

static int file_is_srec(void)
{
 __label__ notsrec;

 int rec( int type __attribute__((__unused__)),
	  int len __attribute__((__unused__)),
	  const unsigned char *data __attribute__((__unused__)) )
  { return(0);
  }

 void err(const char *msg __attribute__((__unused__)))
  { printf("Not an S-record file: %s\n",msg);
    goto notsrec;
  }

 if (filestat.st_size < 14) return(0);
 if (filemap[0] != 'S') return(0);
 parse_srec(filemap,filestat.st_size,&rec,&err);
 return(1);
notsrec:;
 return(0);
}

static FILETYPE guess_filetype(void)
{
 if (file_is_srec()) return(FT_SREC);
 return(FT_BIN);
}

static void segment_bin(void)
{
 nfilesegs = 1;
 filedata = 0;
 filesegs = malloc(sizeof(FILESEG));
 filesegs->base = 0x8c010000;
 filesegs->len = filestat.st_size;
 filesegs->data = filemap;
 haveentry = 1;
 entry = 0x8c010000;
}

static void segment_srec(void)
{
 FILESEG *pfs;

 int rec(int type, int len, const unsigned char *data)
  { FILEDATA *fd;
    unsigned char *dp;
    unsigned int addr;
    if (len < 4) abort();
    switch (type)
     { default:
	  abort();
	  break;
       case 3:
	  addr = (data[0] * 0x01000000) | (data[1] * 0x00010000) |
		 (data[2] * 0x00000100) | data[3];
	  data += 4;
	  len -= 4;
	  if (len < 1) return(0);
	  if (len > FILEDATA_CHUNK) abort();
	  if (!filedata || (filedata->fill+len > FILEDATA_CHUNK))
	   { fd = malloc(sizeof(FILEDATA)+FILEDATA_CHUNK);
	     fd->link = filedata;
	     filedata = fd;
	     fd->fill = 0;
	     pfs = 0;
	   }
	  dp = ((unsigned char *)(filedata+1)) + filedata->fill;
	  bcopy(data,dp,len);
	  filedata->fill += len;
	  if (pfs && (addr == pfs->base+pfs->len))
	   { if (pfs->data+pfs->len != dp) abort();
	     pfs->len += len;
	   }
	  else
	   { filesegs = realloc(filesegs,(nfilesegs+1)*sizeof(FILESEG));
	     pfs = &filesegs[nfilesegs];
	     pfs->base = addr;
	     pfs->len = len;
	     pfs->data = dp;
	     nfilesegs ++;
	   }
	  break;
       case 7:
	  haveentry = 1;
	  entry = (data[0] * 0x01000000) | (data[1] * 0x00010000) |
		  (data[2] * 0x00000100) | data[3];
	  break;
     }
    return(0);
  }

 void err(const char *msg __attribute__((__unused__)))
  { abort();
  }

 nfilesegs = 0;
 filedata = 0;
 filesegs = 0;
 haveentry = 0;
 pfs = 0;
 parse_srec(filemap,filestat.st_size,&rec,&err);
}

static void segment_stats(void)
{
 int i;
 FILESEG *s;

 maxseglen = 0;
 for (i=nfilesegs-1;i>=0;i--)
  { s = &filesegs[i];
    if (s->len > maxseglen) maxseglen = s->len;
  }
 cookiebase = (maxseglen + 1023) >> 10;
 printf("File segments: %d\n",nfilesegs);
 printf("Max segment length: %d\n",maxseglen);
 printf("Cookie base factor: %d\n",cookiebase);
}

static void segment_file(void)
{
 FILETYPE t;

 free(filesegs);
 while (filedata)
  { FILEDATA *f;
    f = filedata;
    filedata = f->link;
    free(f);
  }
 t = arg2type;
 if (t == FT_GUESS) t = guess_filetype();
 switch (t)
  { default:
       abort();
       break;
    case FT_BIN:
       segment_bin();
       break;
    case FT_SREC:
       segment_srec();
       break;
  }
 segment_stats();
}

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);
  }
 filesegs = 0;
 filedata = 0;
 segment_file();
}

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;
 opkt[15] = 0;
 opkt[16] = 0;
 opkt[17] = 0;
 opkt[18] = 0;
 sendopkt(19);
}

static int 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(0);
  }
 if (fstat(fd,&stb) < 0)
  { fprintf(stderr,"%s: can't fstat %s (%s), using cached data\n",
	__progname,arg2,strerror(errno));
    close(fd);
    return(0);
  }
 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(0);
  }
 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(0);
  }
 close(filefd);
 munmap(filemap,filestat.st_size);
 filefd = fd;
 filestat = stb;
 filemap = data;
 segment_file();
 return(1);
}

static void ipkt_0x02(const unsigned char *ip, int pl)
{
 unsigned int cookie;
 unsigned int cookie_seg;
 unsigned int cookie_kws;
 int nb;
 unsigned int addr;

 if (pl < 6+6+2+1+4)
  { printf("0x02 packet too short, ignoring\n");
    return;
  }
 bcopy(ip+6,&opkt[0],6);
 opkt[12] = ip[12];
 opkt[13] = ip[13];
 opkt[14] = 0x03;
 cookie = (ip[15] * 0x01000000) +
	  (ip[16] * 0x00010000) +
	  (ip[17] * 0x00000100) +
	  (ip[18] * 0x00000001);
 bcopy(&ip[15],&opkt[15],4);
 if (freshen_filestat() && cookie)
  { printf("File changed mid-send, sending bad-cookie to %02x:%02x:%02x:%02x:%02x:%02x\n",ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
    bcopy(&ip[15],&opkt[15],4);
    opkt[19] = 0x02;
    sendopkt(20);
    return;
  }
 cookie_seg = cookie / cookiebase;
 cookie_kws = cookie % cookiebase;
 printf("0x02 request, cookie %d, seg %d kws %d\n",cookie,cookie_seg,cookie_kws);
 if ((cookie_seg == nfilesegs) && (cookie_kws == 0))
  { printf("Sending EOF marker to %02x:%02x:%02x:%02x:%02x:%02x\n",ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
    opkt[19] = 0x01;
    opkt[20] =  entry >> 24        ;
    opkt[21] = (entry >> 16) & 0xff;
    opkt[22] = (entry >>  8) & 0xff;
    opkt[23] =  entry        & 0xff;
    sendopkt(24);
    return;
  }
 if ( (cookie_seg >= nfilesegs) ||
      ((cookie_kws * 1024) >= filesegs[cookie_seg].len) )
  { printf("Bad cookie values, sending bad-cookie to %02x:%02x:%02x:%02x:%02x:%02x\n",ip[6],ip[7],ip[8],ip[9],ip[10],ip[11]);
    opkt[19] = 0x02;
    sendopkt(20);
    return;
  }
 nb = filesegs[cookie_seg].len - (cookie_kws * 1024);
 if (nb > 1024) nb = 1024;
 opkt[14] = 0x03;
 bcopy(&ip[15],&opkt[15],4);
 bcopy(filesegs[cookie_seg].data+(cookie_kws*1024),&opkt[30],nb);
 addr = filesegs[cookie_seg].base + (cookie_kws * 1024);
 if ((cookie_kws*1024)+nb < filesegs[cookie_seg].len)
  { cookie_kws ++;
  }
 else
  { cookie_kws = 0;
    cookie_seg ++;
  }
 cookie = (cookie_seg * cookiebase) + cookie_kws;
 opkt[19] = 0x00;
 opkt[20] = (cookie >> 24) & 0xff;
 opkt[21] = (cookie >> 16) & 0xff;
 opkt[22] = (cookie >>  8) & 0xff;
 opkt[23] =  cookie        & 0xff;
 opkt[24] = (addr >> 24) & 0xff;
 opkt[25] = (addr >> 16) & 0xff;
 opkt[26] = (addr >>  8) & 0xff;
 opkt[27] =  addr        & 0xff;
 opkt[28] = (nb >> 8) & 0xff;
 opkt[29] =  nb       & 0xff;
 sendopkt(30+nb);
}

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);
}