#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/uio.h>
#include <net/ethernet.h>
#include <netinet/in_systm.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <netdb.h>
#include <errno.h>
#include <err.h>
#include <fcntl.h>
#include <dnet.h>
#include <pcap.h>
#include <syslog.h>
#include <stdarg.h>

#define BUF_SIZ 256*256

struct sockaddr_in bootp_server;
char errbuf[PCAP_ERRBUF_SIZE];

int open_interfaces(int argc,char **argv);
int create_bootp(char *server);
int create_socket_bootps(void);
int create_socket_answer(void);
int process_bootps(void);
int process_bootps_server(int i,u_char *msgbuf,struct sockaddr_in fromsock,socklen_t socklen);
int process_bootps_client(int len,u_char *data,struct ip *ip);
void pcap_callback(u_char *user, const struct pcap_pkthdr *h, const u_char *sp);

void add_request(char *xid,uint32_t ip);
struct request *find_request(char *xid);

char *print_mac(u_char *s);
char *print_xid(u_char *s);
char *print_ip(u_char *s);
char *print_ip_uint32(uint32_t s);
char *print_dhcptype(u_char *s,int len);
char *print_hostname(u_char *s,int len);

struct request {
    char xid[4];
    int broadcast;
    struct request *next;
};
struct request *requests=NULL;

int DEBUG=0;

int bootpc_port=0;
int bootps_port=0;

struct interface {
    int number;
    char name[INTF_NAME_LEN];
    ip_addr_t ip;
    int mask;
    eth_addr_t mac;
    eth_t *fd;
    int fds;
    pcap_t *cap;
};
struct interface *interfaces,*interface;

char *wait=".oOo";

int main(int argc,char **argv) {
    int waitcounter=0;

    while (argc>1 && strcmp(argv[1],"-d")==0) {
	DEBUG++;
	argv++;
	argc--;
    }

    if (DEBUG==0) {
	pid_t p;
	if (fork()) {
	    sleep(3);
	    exit(0);
	}
	if ((p=fork())!=0) {
	    FILE *f;
	    openlog("dchprelay",LOG_NDELAY|LOG_PID,LOG_DAEMON);
	    syslog(LOG_NOTICE,"dhcprelay started as process %d",p);
	    if ((f=fopen("/var/run/dhcprelay.pid","w"))!=NULL) {
		fprintf(f,"%d\n",p);
		fclose(f);
	    }
	    sleep(3);
	    exit(0);
	}
    }
    sleep(1);

    {
	struct servent *servent;
	if ((servent=getservbyname("bootps",0))==NULL)
	    errx(1,"getservbyname(%s)","bootps");
	bootps_port=servent->s_port;
	if ((servent=getservbyname("bootpc",0))==NULL)
	    errx(1,"getservbyname(%s)","bootpc");
	bootpc_port=servent->s_port;
    }

    openlog("dchprelay",LOG_NDELAY|LOG_PID,LOG_DAEMON);

    if (argc==1) {
	printf(
	    "Usage: %s [-d] <interface[:ipaddress]> <dhcpserver>\n"
	    "Example: %s fxp0 10.192.0.1\n"
	    "Example: %s fxp0:192.168.1.1 10.192.0.1\n",
	    argv[0],argv[0],argv[0]
	);
	return 0;
    }

    open_interfaces(argc,argv);
    create_bootp(argv[argc-1]);

    while (1) {
	u_char i;
	if (DEBUG) fprintf(stderr,"%c",wait[waitcounter++%4]);

	i=0;
	while (interfaces[i].number) {
	    interface=&interfaces[i];
	    pcap_dispatch(interface->cap,1,pcap_callback,NULL);

	    {
		fd_set read;
		struct timeval timeout;
		int max;

		FD_ZERO(&read);
		FD_SET(interface->fds,&read);
		max=interface->fds;
		timeout.tv_sec=0;
		timeout.tv_usec=10;
		if (select(max+1,&read,NULL,NULL,&timeout)<0) {
		    errx(1,"select");
		}
		if (FD_ISSET(interface->fds,&read)) {
		    process_bootps();
		    FD_CLR(interface->fds,&read);
		}
	    }

	    i++;
	}
    }

    closelog();

    return 0;
}

void pcap_callback(u_char *user, const struct pcap_pkthdr *h, const u_char *sp) {
    struct ether_header *eh;
    struct ip *ip;
    struct udphdr *udp;
    u_char *data;
    int offset=0;

    if (DEBUG>1)
	printf("%s - %d/%d - ",interface->name,h->caplen,h->len);

    if (h->caplen<ETHER_HDR_LEN) {
	// silently ignore
	syslog(LOG_WARNING,"eh:caplen(%d)<%d",h->caplen,ETHER_HDR_LEN);
	return;
    }

    eh=(struct ether_header *)(sp+offset);
    offset+=ETHER_HDR_LEN;
    if (DEBUG>1) {
	printf(" %s",print_mac(eh->ether_dhost));
	printf(" %s",print_mac(eh->ether_shost));
	printf(" %d",eh->ether_type);
    }
    // check for IPv4 packets
    if (eh->ether_type!=8) { 
	if (DEBUG>1) printf("\n");
	return;
    }

    if (h->caplen<offset+sizeof(struct ip)) {
	// silently ignore
	syslog(LOG_WARNING,"ip:caplen(%d)<%d",
	    h->caplen,offset+sizeof(struct ip)
	);
	return;
    }

    ip=(struct ip *)(sp+offset);
    offset+=sizeof(struct ip);
    if (DEBUG>1) {
	printf(" - %s",print_ip_uint32((uint32_t)(ip->ip_src.s_addr)));
	printf(" %s",print_ip_uint32((uint32_t)(ip->ip_dst.s_addr)));
    }
    if (ip->ip_p!=IPPROTO_UDP) {
	if (DEBUG>1) printf("\n");
	return;
    }

    if (h->caplen<offset+sizeof(struct udphdr)) {
	// silently ignore
	syslog(LOG_WARNING,"udp:caplen(%d)<%d",
	    h->caplen,offset+sizeof(struct udphdr)
	);
	return;
    }

    udp=(struct udphdr *)(sp+offset);
    offset+=sizeof(struct udphdr);

    data=(u_char *)(sp+offset);
    if (DEBUG>2) {
	printf(" - %d %d %d",data[0],data[1],data[2]);
	printf("\n");
    }

    if (h->caplen<offset+240) {
	// silently ignore
	syslog(LOG_WARNING,"dhcp:caplen(%d)<%d",
	    h->caplen,offset+240
	);
	return;
    }

    if (DEBUG>1)
	printf("\n");

    process_bootps_client(h->caplen-offset,data,ip);
}

int open_interfaces(int argc,char **argv) {
    int i;
    intf_t *intf;

    interfaces=(struct interface *)calloc(argc,sizeof(struct interface));
    intf=intf_open();
    for (i=1;i<argc-1;i++) {

	char *ifname;
	char *ipaddress=NULL;
	char *p;

	ifname=argv[i];
	if ((p=strchr(argv[i],':'))!=NULL) {
	    *p=0;
	    p++;
	    ipaddress=p;
	}

	{
	    struct intf_entry ifentry;
	    ip_addr_t ip_src;
	    char msg[BUF_SIZ];

	    memset(&ifentry,0,sizeof(struct intf_entry));
	    strcpy(ifentry.intf_name,ifname);
	    intf_get(intf,&ifentry);

	    interfaces[i-1].number=i;
	    strcpy(interfaces[i-1].name,argv[i]);
	    if (ipaddress) {
		struct hostent *hostent;
		if ((hostent=gethostbyname(ipaddress))==NULL) 
		    errx(1,"gethostbyname(%s)\n",ipaddress);
		memcpy(&interfaces[i-1].ip,hostent->h_addr,hostent->h_length);
	    } else {
		ip_src=ifentry.intf_addr.addr_ip;
		memcpy(&interfaces[i-1].ip,&ip_src,sizeof(ip_addr_t));
	    }
	    interfaces[i-1].mask=ifentry.intf_addr.addr_bits;
	    memcpy(&interfaces[i-1].mac,&ifentry.intf_link_addr.addr_eth,sizeof(eth_addr_t));
	    interfaces[i-1].fd=eth_open(ifname);
	    sprintf(msg,"Opening %s %s/%d, %s",
		ifname,
		print_ip((u_char *)&interfaces[i-1].ip),
		interfaces[i-1].mask,
		print_mac((u_char *)&interfaces[i-1].mac));
	    syslog(LOG_NOTICE,"%s",msg);
	    fprintf(stderr,"%s\n",msg);
	}

	{
	    pcap_t *cap;
	    struct bpf_program fp;

	    if ((cap=pcap_open_live(ifname,1500,1,100,errbuf))==NULL)
		errx(1,"pcap_open_live(%s): %s",ifname,errbuf);
	    if (pcap_compile(cap,&fp,"udp and (port bootpc or port bootps)",0,0)<0)
		errx(1,"pcap_compile: %s",pcap_geterr(cap));
	    if (pcap_setfilter(cap,&fp)<0)
		errx(1,"pcap_setfilter: %s",pcap_geterr(cap));
	    if (pcap_setnonblock(cap,0,errbuf)<0)
		errx(1,"pcap_setnonblock(%s): %s",ifname,pcap_geterr(cap));
	    interfaces[i-1].cap=cap;
	}

	{
	    int x;
	    struct sockaddr_in name;
	    int fd;

	    if (DEBUG>1) printf("Creating bootps socket.\n");

	    if ((fd=socket(PF_INET,SOCK_DGRAM,IPPROTO_UDP))<0)
		errx(1,"socket");

	    x=1;
	    if (setsockopt(fd,SOL_SOCKET,SO_BROADCAST,(char *)&x,sizeof(x))<0)
		errx(1,"setsockopt: SO_BROADCAST");
	    if (setsockopt(fd,SOL_SOCKET,SO_REUSEADDR,(char *)&x,sizeof(x))<0)
		errx(1,"setsockopt: SO_REUSEADDR");

	    name.sin_family = AF_INET;
	    name.sin_port = bootps_port;
	    memcpy(&name.sin_addr.s_addr,&interfaces[i-1].ip,4);

	    if (bind(fd,(struct sockaddr *)&name,sizeof name)<0)
		errx(1,"bind failed.");
	    interfaces[i-1].fds=fd;
	}
    }
    intf_close(intf);

    return 0;
}

int create_bootp(char *server) {
    struct hostent *hostent;

    if ((hostent=gethostbyname(server))==NULL) 
	errx(1,"gethostbyname(%s)",server);

    bootp_server.sin_family=AF_INET;
    bootp_server.sin_port=bootps_port;
    memcpy(&bootp_server.sin_addr.s_addr,hostent->h_addr,hostent->h_length);
    return 0;
}

int process_bootps() {
    u_char msgbuf[BUF_SIZ];
    struct sockaddr_in fromsock;
    socklen_t fromlen=sizeof(fromsock);
    int i;

    if (DEBUG>1) printf("process_bootps\n");

    i=recvfrom(interface->fds,msgbuf,BUF_SIZ,0,(struct sockaddr *)&fromsock,&fromlen);
    // silently ignore requests, they are handled via pcap
    if (msgbuf[0]!=2)
	return 0;

    if (DEBUG>1) printf("%d bytes from %s\n",i,print_ip((u_char *)&fromsock.sin_addr));

    process_bootps_server(i,msgbuf,fromsock,fromlen);
    if (DEBUG>1) printf("\n");
    return 0;
}

int process_bootps_server(int msgbuf_len,u_char *msgbuf,struct sockaddr_in fromsock,socklen_t socklen) {
    char packet[2*BUF_SIZ];
    struct request *r;
    int packet_offset=0;
    struct in_addr *in;

    struct ether_header *eh;
    int eh_len;
    struct ip *ip;
    int ip_len;
    struct udphdr *udp;
    int udp_len;
    char *hostname;
    char syslogmsg[BUF_SIZ];

    int result;

    if (DEBUG>1) printf("process_bootps_server\n");

    in=(struct in_addr *)(msgbuf+16);

    r=find_request(msgbuf+4);
    if (r==NULL) {
	printf("Question not found (%s)\n",print_xid(msgbuf));
	return 0;
    }

    hostname=print_hostname(msgbuf,msgbuf_len);
    sprintf(syslogmsg,"%s from %s for %s%s%s%s",
	    print_dhcptype(msgbuf,msgbuf_len),
	    print_ip((u_char *)&fromsock.sin_addr),
	    print_mac(msgbuf+28),
	    hostname==NULL?"":" (",
	    hostname==NULL?"":hostname,
	    hostname==NULL?"":")"
	    );
    sprintf(syslogmsg+strlen(syslogmsg)," to %s/%s",
	    interface->name,
	    print_ip((u_char *)in)
	    );
    if (r->broadcast)
	sprintf(syslogmsg+strlen(syslogmsg)," via 255.255.255.255");
    syslog(LOG_INFO,"%s",syslogmsg);
    if (DEBUG) printf("%s\n",syslogmsg);

    {
	eh=(struct ether_header *)(packet+packet_offset);
	memcpy(eh->ether_dhost,msgbuf+28,ETHER_ADDR_LEN);
	memcpy(eh->ether_shost,&(interfaces->mac),ETHER_ADDR_LEN);
	eh->ether_type=htons(ETHERTYPE_IP);

	eh_len=ETHER_HDR_LEN;
	packet_offset+=eh_len;
    }
    {
	ip_addr_t ip_src=interface->ip;

	ip=(struct ip *)(packet+packet_offset);
	ip->ip_v=4;
	ip->ip_hl=20/4;
	ip->ip_tos=IPTOS_LOWDELAY;
	ip->ip_len=htons(sizeof(struct ip)+sizeof(struct udphdr)+msgbuf_len);
	ip->ip_id=0;
	ip->ip_off=0;
	ip->ip_ttl=16;
	ip->ip_p=IPPROTO_UDP;
	ip->ip_sum=0;

	memcpy(&ip->ip_src,&ip_src,4);

	if (r->broadcast)
	    ip->ip_dst.s_addr=INADDR_BROADCAST;
	else
	    memcpy(&ip->ip_dst,in,4);

	ip_len=sizeof(struct ip);
	packet_offset+=ip_len;
    }
    {
	udp=(struct udphdr *)(packet+packet_offset);
	udp->uh_sport=bootps_port;
	udp->uh_dport=bootpc_port;
	udp->uh_ulen=htons(sizeof(struct udphdr)+msgbuf_len);
	udp->uh_sum=0;

	udp_len=sizeof(struct udphdr);
	packet_offset+=udp_len;
    }
    memcpy(packet+packet_offset,msgbuf,msgbuf_len);
    packet_offset+=msgbuf_len;

    ip_checksum(ip,ip_len+udp_len+msgbuf_len);

    result=eth_send(interface->fd,packet,packet_offset);
    if (result==-1)
	warnx("Not everything was sent");


    return 0;
}

int process_bootps_client(int len,u_char *data,struct ip *ip) {
    char *hostname;
    char syslogmsg[BUF_SIZ];

    if (DEBUG>1) printf("process_bootps_client\n");

    // ignore replies
    if (data[0]!=1) return 0;

    data[3]++;					// hops
    memcpy(data+24,&interface->ip,4);		// GIADDR

    hostname=print_hostname(data,len);
    sprintf(syslogmsg,"%s from %s/%s for %s%s%s%s",
	    print_dhcptype(data,len),
	    interface->name,
	    print_ip_uint32((uint32_t)(ip->ip_src.s_addr)),
	    print_mac(data+28),
	    hostname==NULL?"":" (",
	    hostname==NULL?"":hostname,
	    hostname==NULL?"":")"
	    );
    sprintf(syslogmsg+strlen(syslogmsg)," to %s",
	    print_ip_uint32((uint32_t)(ip->ip_dst.s_addr))
	    );
    sprintf(syslogmsg+strlen(syslogmsg)," forwarded to %s",
	    print_ip((u_char *)&bootp_server.sin_addr)
	    );
    syslog(LOG_INFO,"%s",syslogmsg);
    if (DEBUG) printf("%s\n",syslogmsg);

    sendto(interface->fds,data,len,0,(struct sockaddr *)&bootp_server,sizeof(bootp_server));
    add_request(data+4,(uint32_t)(ip->ip_dst.s_addr));

    return 0;
}


void add_request(char *xid,uint32_t ip) {
    struct request *r;

    r=find_request(xid);
    if (r==NULL) {
	r=(struct request *)calloc(1,sizeof(struct request));
	memcpy(r->xid,xid,4);
	r->next=requests;
	requests=r;
    }

    if (ip==-1) r->broadcast=1;
}

struct request *find_request(char *xid) {
    struct request *r;

    for (r=requests;r!=NULL;r=r->next) {
	if (memcmp(r->xid,xid,4)==0) return r;
    }

    return NULL;
}


char *print_xid(u_char *s) {
    static char buffer[BUF_SIZ];
    memset(buffer,0,BUF_SIZ);
    sprintf(buffer,"%02x%02x%02x%02x",
	s[0],s[1],s[2],s[3]);
    return buffer;
}

char *print_mac(u_char *s) {
    static char buffer[BUF_SIZ];
    memset(buffer,0,BUF_SIZ);
    sprintf(buffer,"%02x:%02x:%02x:%02x:%02x:%02x",
	s[0],s[1],s[2],s[3],s[4],s[5]);
    return buffer;
}

char *print_ip(u_char *s) {
    static char buffer[BUF_SIZ];
    memset(buffer,0,BUF_SIZ);
    sprintf(buffer,"%d.%d.%d.%d",s[0],s[1],s[2],s[3]);
    return buffer;
}

char *print_ip_uint32(uint32_t s) {
    static char buffer[BUF_SIZ];
    u_char b[4];
    memset(buffer,0,BUF_SIZ);
    memcpy(b,&s,4);
    sprintf(buffer,"%u.%u.%u.%u",b[0],b[1],b[2],b[3]);
    return buffer;
}

u_char *find_option(u_char *s,int maxlen,int type) {
    int len=maxlen;
    while (s[0]!=0 && len>=0) {
	if (s[0]==type) {	// DHCP Message Type
	    return s;
	}
	s+=2+s[1];
	len-=2+s[1];
    }
    return NULL;
}

char *print_dhcptype(u_char *s,int len) {
    static char buffer[BUF_SIZ];
    static char *types[8]={
	"DISCOVER",
	"OFFER",
	"REQUEST",
	"DECLINE",
	"ACK",
	"NAK",
	"RELEASE",
	"INFORM"
    };
    memset(buffer,0,BUF_SIZ);
    s+=240;
    if ((s=find_option(s,len-240,53))==NULL) {
	strcpy(buffer,"not-found");
	return buffer;
    }
    if (s[2]>=1 && s[2]<=8)
	strcpy(buffer,types[s[2]-1]);
    else
	sprintf(buffer,"UNKOWN-%d",s[2]);
    return buffer;
}

char *print_hostname(u_char *s,int len) {
    static char buffer[BUF_SIZ];
    memset(buffer,0,BUF_SIZ);
    s+=240;
    if ((s=find_option(s,len-240,12))==NULL) return NULL;
    strncpy(buffer,s+2,s[1]);
    return buffer;
}