/*
 * ethernet frame generator. (C) 2000-2007 Willy Tarreau <willy@ant-computing.com>
 * Use and redistribute under terms of GPL.
 *
 * WARNING: no not use when you don't know what this is intended to do, this could
 * kill your network within a few tens of seconds.
 *
 * Best results seen with :
 *
 *  - DEC 21143 (DEC Tulip, Dlink DFE570TX) : >148 kpps, no error.
 *  - SIS900 : idem, but high system overhead.
 *  - Adaptec Starfire ANA62044 (aha6915) : >148kpps, some errors, particularly
 *    when link goes down.
 *  - Intel EEPro100 : slightly less, a few unpredictable errors.
 *  - 3Com 3c905C : 82 kpps, no error, little load on the system.
 *  - Realtek RTL8139 : system hangs immediately, never use it !!!
 */

// please read "man 7 packet"
#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <time.h>
#include <sys/socket.h>
#include <features.h>    /* for the glibc version number */
#include <net/if.h>
#include <sys/types.h>
#include <linux/sockios.h>
#if __GLIBC__ >= 2 && __GLIBC_MINOR >= 1
#include <netpacket/packet.h>
#include <net/ethernet.h>     /* the L2 protocols */
#else
#include <linux/if_packet.h>
#include <linux/if_ether.h>   /* The L2 protocols */
#include <sys/time.h>
#endif
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/ip_icmp.h>
#include <sys/ioctl.h>


#define ETH_HW_ADDR_LEN 6
#define IP_ADDR_LEN 4
#define ETHER_HW_TYPE 1
#define IP_PROTO_TYPE 0x0800
#define ICMP_DATA_LEN 56

#if (ICMP_DATA_LEN<22)
#define PADDING (22-ICMP_DATA_LEN)
#else
#define PADDING 0
#endif

#define DEFAULT_DEVICE "eth0"

#define MAX_FRAME_SIZE 9018	/* up to jumbo frames */

struct s_eth_hdr {
	/* ETHERNET header */
	u_char dst[ETH_HW_ADDR_LEN];
	u_char src[ETH_HW_ADDR_LEN];
};


struct s_vlan {
	u_short	proto;	/* 0x8100 */
	u_short	id;	/* 4 flags + vlan_id */
};


struct s_ip_hdr {
	/* IP header */
	u_char	verihl;	/* 0x45 */
	u_char	tos;
	u_short	totlen;
	u_short	id;
	u_short	fragoff;
	u_char	ttl;
	u_char	protocol;
	u_short	check;
	u_char	src[IP_ADDR_LEN];
	u_char	dst[IP_ADDR_LEN];
};


struct s_icmp_hdr {
	u_char	type;				/* 0x08 = ICMP_ECHOREQUEST */
	u_char	code;				/* 0x00 (ignored) */
	u_short	icmpchk;
	union {
		struct {
			u_short	id;
			u_short	sequence;
		} echo;
		u_char	gw_addr[IP_ADDR_LEN];
	} un;
};


struct s_udp_hdr {
	u_short	src;
	u_short	dst;
	u_short	len;
	u_short	chk;
};

void *p_frame;	/* pointer to the frame itself */
void *p_l2;	/* pointer to the end of the encapsulation if any (vlan...) */
void *p_l3;	/* pointer to the L3 protocol header */
void *p_l4;	/* pointer to the L4 protocol header (tcp, udp, icmp -although not l4- ) */
void *p_data;	/* L4 proto data */

void die(char *);
void get_ip_addr(struct in_addr *, char *);
void get_hw_addr(char *, char *);

void die(char* str){
	fprintf(stderr,"%s\n",str);
	exit(1);
}

void get_ip_addr(struct in_addr* in_addr, char* str){
	struct hostent *hostp;

	in_addr->s_addr = inet_addr(str);
	if (in_addr->s_addr == -1) {
		if ((hostp = gethostbyname(str)))
			bcopy (hostp->h_addr, in_addr, hostp->h_length);
		else {
			*(unsigned long *)in_addr = -1;  /* 255.255.255.255 */
		}
	}
}

void get_hw_addr(char* buf, char* str){

	int i;
	char c, val = 0;

	for(i = 0; i < ETH_HW_ADDR_LEN; i++){
		while ((*str == ':') || (*str == '.'))
			str++;
		if (!(c = tolower(*str++)))
			die("Invalid hardware address");
		if (isdigit(c))
			val = c-'0';
		else if(c >= 'a' && c <= 'f')
			val = c-'a'+10;
		else
			die("Invalid hardware address");

		*buf = val << 4;
		if( !(c = tolower(*str++))) die("Invalid hardware address");
		if(isdigit(c)) val = c-'0';
		else if(c >= 'a' && c <= 'f') val = c-'a'+10;
		else die("Invalid hardware address");

		*buf++ |= val;

		if(*str == ':')str++;
	}
}

/* just like strcat, but with packets */
static inline u_char *pktcat(u_char *to, void *from, int len) {
	memcpy(to, from, len);
	return to+len;
}

/* returns a big-endian checksum of the buffer */
unsigned short checksum(unsigned short* addr, int len) {
	register long sum = 0;
  
	while(len > 1){
		sum += *addr++;
		len -= 2;
	}

	if (len & 1)
		sum += (*addr & 0xff);

	sum = (sum & 65535) + (sum >> 16);

	return ~sum;
}

/* returns a big-endian checksum of the buffer */
unsigned short l4_checksum(unsigned long src, unsigned long dst, u_char proto, unsigned short* addr, int len) {
	register long sum = 0;
  
	sum = (src & 0xffff) + (src >> 16) + (dst & 0xffff) + (dst >> 16);
	sum += (proto << 8); sum += htons(len);

	while(len > 1){
		sum += *addr++;
		len -= 2;
	}

	if (len & 1)
		sum += (*addr & 0xff);

	sum = (sum & 65535) + (sum >> 16);

	return ~sum;
}

void usage() {
	printf("ethforge : Ethernet frame forger v20070126 - (c) Willy Tarreau 2000-2007\n"
	       "Syntaxe: ethforge [ -g ] [ -r ] [ -h ] [ -n packets ] [ -w wait_us ] [ -t ttl ]\n"
	       "                  [ -i id ] [ -l len ] [ -S l2src ] [ -D l2dst] [ -e device ]\n"
	       "                  [ -v vlan ]* [ -p l3proto ] [ -s l3src ] [ -d l3dst ] [ -m ]\n"
	       "                  [ -x l3srcmask ] [ -y l3dstlask ] [ -m ]\n"
	       "                  [ -u udp_dst_p ] [ -U udp_src_p ] [ -f ] [ -F ]\n"
	       "                  [ -W ] [ -E ] [ -o frag_offset ] [ -P l4proto ]\n"
	       "                  [ -L advlen ]\n"
	       "  -o sets frag offset. -1 for random ; same for id.\n"
	       "  -g means that data won't be generated, but 'l' bytes will be read from stdin\n"
	       "     and sent after vlan and l3proto. Read hexa if -G.\n"
	       "  -r means to randomize a bit the wait time.\n"
	       "  -m sets random l2dst to multicast.\n"
	       "  -f = don't fragment.\n"
	       "  -F = more fragments.\n"
	       "  -W = wait via select() ; -E = ignore errors\n"
	       "  -L = force adv size ; -T = fast wait loop iterations\n"
	       "  if no l2 addr is set, random values will be used for each frame.\n"
	       "\n");
	exit(1);
}

/* make a random addr, up to 64 bits long (len bytes) */
void rand_addr(void *to, int len) {
	unsigned long addr[2];
	addr[0] = random(); addr[1] = random();
	memcpy(to, addr, len);
}


int get_hexbyte() {
	int a, b = -1;
	while ((a = toupper(getchar())) != EOF) {
		if ((a < '0') || (a > 'F'))
			continue;
		if ((a >= '0') && (a <= '9'))
			a -= '0';
		else
			a -= 'A' - 10;
		if (b<0)
			b = a << 4;
		else
			return b | a;
	}
	return EOF;
}

int main(int argc,char** argv){

	int loops = 0;
	int waittime = 0;
	int datalen = ICMP_DATA_LEN, advlen = -1;
	char *arg_l3_src = NULL, *arg_l3_dst = NULL,
		*arg_l2_src = NULL, *arg_l2_dst = NULL,
		*arg_l3_srcmask = NULL, *arg_l3_dstmask = NULL,
		*arg_device = DEFAULT_DEVICE;
	int arg_random_delay = 0, arg_get_data = 0, arg_get_hex = 0, arg_mcast = 0, arg_fragoff = 0;
	int arg_dontfrag = 0, arg_morefrag = 0, arg_waitselect = 0, arg_ignore=0;
	int arg_loops = 0;
	int manual_l4 = 0;
	int pktid = -2, ttl = 64;
	int sudp = -1, dudp = -1;
	int l4proto = IPPROTO_ICMP;
	void *input_data = NULL;

	int vlan = 0;
	int proto = 0;
	struct sockaddr_ll sa;
	struct ifreq ifr;

	fd_set wfd;
	int sock;
	int i;
   
	long l3src=0, l3dst=0, l3sm = 0xffffffff, l3dm = 0xffffffff;

	srandom(getpid() * time(NULL));
 
	p_frame = (void *)calloc(1, MAX_FRAME_SIZE);
	p_l2 = p_frame + sizeof(struct s_eth_hdr);

	if (argc <= 1)
		usage();

	argc--; argv++;
	while (argc > 0) {
		char *flag;

		if (**argv == '-') {
			flag = *argv+1;

			/* 1 arg */
			if (*flag == 'h')
				usage();
			else if (*flag == 'r')
				arg_random_delay = 1;
			else if (*flag == 'g')
				arg_get_data = 1;
			else if (*flag == 'G')
				arg_get_data = arg_get_hex = 1;
			else if (*flag == 'm')
				arg_mcast = 1;
			else if (*flag == 'f')
				arg_dontfrag = 1;
			else if (*flag == 'F')
				arg_morefrag = 1;
			else if (*flag == 'W')
				arg_waitselect = 1;
			else if (*flag == 'E')
				arg_ignore = 1;
			else { /* 2+ args */
				argv++; argc--;
				if (argc == 0)
					usage();

				switch (*flag) {
				case 'n' : loops = atol(*argv); break;
				case 'w' : waittime = atol(*argv); break;
				case 't' : ttl = atol(*argv); break;
				case 'i' : pktid = atol(*argv); break;
				case 'l' : datalen = atol(*argv); break;
				case 'L' : advlen = atol(*argv); break;
				case 'T' : arg_loops = atol(*argv); break;
				case 'x' : arg_l3_srcmask = *argv; l3sm = inet_addr(arg_l3_srcmask); break;
				case 'y' : arg_l3_dstmask = *argv; l3dm = inet_addr(arg_l3_dstmask); break;
				case 's' : arg_l3_src = *argv; l3src = inet_addr(arg_l3_src);  break;
				case 'S' : arg_l2_src = *argv; break;
				case 'd' : arg_l3_dst = *argv; l3dst = inet_addr(arg_l3_dst);  break;
				case 'D' : arg_l2_dst = *argv; break;
				case 'e' : arg_device = *argv; break;
				case 'o' : arg_fragoff = atol(*argv); break;
				case 'p' : proto = atol(*argv); break;
				case 'P' : {
					l4proto = atol(*argv);
					manual_l4 = 1;
					break;
				}
				case 'u' : {
					l4proto = IPPROTO_UDP;
					dudp = atol(*argv);
					break;
				}
				case 'U' : {
					l4proto = IPPROTO_UDP;
					sudp = atol(*argv);
					break;
				}
				case 'v' : { /* allow multiple vlan encapsulations */
					vlan = atol(*argv);
					((struct s_vlan *)p_l2)->proto = htons(0x8100);/* vlan protocol */
					((struct s_vlan *)p_l2)->id = htons(vlan);  	/* vlan id + flags */
					p_l2 += sizeof(struct s_vlan);
					break;
				}
				default: usage();
				}
			}
		}
		else
			usage();
		argv++; argc--;
	}

	if (arg_l2_dst)
		get_hw_addr(((struct s_eth_hdr *)p_frame)->dst, arg_l2_dst);
	else
		if (arg_mcast) /* in case of random addresses, perhaps we have to set multicast */
			*(char *)((struct s_eth_hdr *)p_frame)->dst = 1;

	if (arg_l2_src)
		get_hw_addr(((struct s_eth_hdr *)p_frame)->src, arg_l2_src);

	if (pktid < -1)
		pktid = getpid();

	sock = socket(PF_PACKET, SOCK_RAW, (proto==0)?htons(ETH_P_IP):htons(proto));

	if (sock < 0) {
		perror("socket");
		exit(1);
	}

	fcntl(sock, F_SETFL, O_NONBLOCK);
	bzero(&ifr, sizeof(ifr));
	strncpy(ifr.ifr_name, arg_device, sizeof(ifr.ifr_name));
	if (ioctl(sock, SIOCGIFINDEX, &ifr) == -1)
		perror("ioctl()");

	bzero(&sa, sizeof(sa));
	sa.sll_family=AF_PACKET;
	sa.sll_ifindex=ifr.ifr_ifindex;
	bind(sock, (struct sockaddr *)&sa, sizeof(sa));

	p_l3=NULL;
	if (proto == 0)
		*(u_short *)p_l2 = htons(ETH_P_IP);
	else
		*(u_short *)p_l2 = htons(proto);
	p_l2 += 2;

	if (advlen < 0)
		advlen = datalen;

	while(1) {
		if (proto == 0) { /* unspecified, let's send simple IP/ICMP datagrams */
			//*(u_short *)p_l2 = htons(ETH_P_IP);
			if (p_l3 == NULL)
				p_l3 = p_l2;

			((struct s_ip_hdr *)p_l3)->verihl   = 0x45;
			((struct s_ip_hdr *)p_l3)->tos      = 0x00;
			if (!manual_l4 && (l4proto == IPPROTO_ICMP))
				((struct s_ip_hdr *)p_l3)->totlen   = htons(sizeof(struct s_ip_hdr) + sizeof(struct s_icmp_hdr) + advlen);
			else if (!manual_l4 && (l4proto == IPPROTO_UDP))
				((struct s_ip_hdr *)p_l3)->totlen   = htons(sizeof(struct s_ip_hdr) + sizeof(struct s_udp_hdr) + advlen);
			else
				((struct s_ip_hdr *)p_l3)->totlen   = htons(sizeof(struct s_ip_hdr) + advlen);
	    
			if (pktid >= 0)
				((struct s_ip_hdr *)p_l3)->id       = htons(pktid);
			else
				((struct s_ip_hdr *)p_l3)->id       = random();

			if (arg_fragoff >= 0)
				((struct s_ip_hdr *)p_l3)->fragoff  = htons((arg_dontfrag?0x4000:0) | (arg_morefrag?0x2000:0) | arg_fragoff);
			else
				((struct s_ip_hdr *)p_l3)->fragoff  = htons((arg_dontfrag?0x4000:0) | (arg_morefrag?0x2000:0) | (random() & 0x3fff));

			((struct s_ip_hdr *)p_l3)->ttl      = ttl;
			((struct s_ip_hdr *)p_l3)->protocol = l4proto;
			((struct s_ip_hdr *)p_l3)->check    = 0;
	
			if (arg_l3_src != NULL) {
				if (l3sm == 0xffffffff)
					*(long *)&((struct s_ip_hdr *)p_l3)->src = l3src;
				else {
					*(long *)&((struct s_ip_hdr *)p_l3)->src = (l3src & l3sm) | (random() & ~l3sm);
				}
			}
			else {
				rand_addr(&((struct s_ip_hdr *)p_l3)->src, sizeof(struct in_addr));
				*(char *)(&((struct s_ip_hdr *)p_l3)->src) &= 0xDF;
			}

			if (arg_l3_dst != NULL) {
				if (l3dm == 0xffffffff)
					*(long *)&((struct s_ip_hdr *)p_l3)->dst = l3dst;
				else {
					*(long *)&((struct s_ip_hdr *)p_l3)->dst = (l3dst & l3dm) | (random() & ~l3dm);
				}
			}
			else
				rand_addr(&((struct s_ip_hdr *)p_l3)->dst, sizeof(struct in_addr));

			((struct s_ip_hdr *)p_l3)->check = checksum((unsigned short*)p_l3, 20);
			p_l4 = p_l3 + sizeof(struct s_ip_hdr);

			/* initialization of ICMP packet */
			if (!manual_l4 && (l4proto == IPPROTO_ICMP)) {
				((struct s_icmp_hdr*)p_l4)->type = ICMP_ECHO;
				((struct s_icmp_hdr*)p_l4)->code = 0x00;
				((struct s_icmp_hdr*)p_l4)->icmpchk = 0;
				((struct s_icmp_hdr*)p_l4)->un.echo.id = pktid;
				((struct s_icmp_hdr*)p_l4)->un.echo.sequence = htons(0x0000);
				p_data = p_l4 + sizeof(struct s_icmp_hdr);
			}
			else if (!manual_l4 && (l4proto == IPPROTO_UDP)) {
				/* initialization of UDP packet */
				((struct s_udp_hdr*)p_l4)->dst = (dudp == -1) ? random() : htons(dudp);
				((struct s_udp_hdr*)p_l4)->src = (sudp == -1) ? random() : htons(sudp);
				((struct s_udp_hdr*)p_l4)->len = htons(datalen + sizeof(struct s_udp_hdr));
				((struct s_udp_hdr*)p_l4)->chk = 0;

				p_data = p_l4 + sizeof(struct s_udp_hdr);
			}
			else
				p_data = p_l4;

			if (arg_get_data) {
				if (!input_data) {
					input_data = calloc(1, datalen);
					if (arg_get_hex) {
						for (i = 0; i < datalen; i++)
							*(u_char *)(input_data+i) = get_hexbyte();
					} else {
						for (i = 0; i < datalen; i++) {
							*(u_char *)(input_data+i) = getchar();
						}
					}
				}
				memcpy(p_data, input_data, datalen);
				p_data += datalen;
			}
			else
				for (i = 0; i < datalen; i++)
					*(u_char *)p_data++ = i;
	
			if (!manual_l4 && (l4proto == IPPROTO_ICMP))
				((struct s_icmp_hdr*)p_l4)->icmpchk = checksum((unsigned short*)p_l4, p_data - p_l4);
			else if (!manual_l4 && (l4proto == IPPROTO_UDP)) { /* this needs to include parts of IP hdr, see rfc768 */
				((struct s_udp_hdr*)p_l4)->chk = l4_checksum(
									     *(unsigned long *)((struct s_ip_hdr *)p_l3)->src,
									     *(unsigned long *)((struct s_ip_hdr *)p_l3)->dst,
									     l4proto,
									     (unsigned short*)p_l4, p_data - p_l4);
			}
		}
		else {
			p_l3 = p_l2;
			p_l4 = p_l3;
			p_data = p_l4;
			if (arg_get_data) {
				if (!input_data) {
					input_data = calloc(1, datalen);
					if (arg_get_hex) {
						for (i = 0; i < datalen; i++)
							*(u_char *)(input_data+i) = get_hexbyte();
					} else {
						for (i = 0; i < datalen; i++) {
							*(u_char *)(input_data+i) = getchar();
						}
					}
				}
				memcpy(p_data, input_data, datalen);
				p_data += datalen;
			}
			else {
				for (i = 0; i < datalen; i++)
					*(u_char *)p_data++ = random();
			}
		}
    
		/* padding */
#if PADDING
		bzero(pkt.padding,sizeof(pkt.padding));
#endif

		//    strcpy(sa.sa_data, arg_device);

		/* loop indefinitely if loops<0, or "loops" times */
		//    while (1) {
		if (arg_l2_src == NULL)
			rand_addr(((struct s_eth_hdr *)p_frame)->src + 1, ETH_HW_ADDR_LEN - 1);

		if (arg_l2_dst == NULL)
			rand_addr(((struct s_eth_hdr *)p_frame)->dst + 1, ETH_HW_ADDR_LEN - 1);

		/* painfully slow interfaces sometimes need select... */
		if (arg_waitselect) {
			FD_ZERO(&wfd);
			FD_SET(sock, &wfd);
			select(sock+1, NULL, &wfd, NULL, NULL); /* wait for the socket buffers to be free */
		}
		//if ((send(sock, p_frame, p_data - p_frame, 0) < 0) && !arg_ignore) {
		//	perror("sendto");
		//	exit(1);
		//}
		if ((send(sock, p_frame, p_data - p_frame, MSG_NOSIGNAL | MSG_DONTWAIT) < 0) &&
		    !arg_ignore && (errno != EAGAIN)) {
			perror("sendto");
			exit(1);
		}
		//if ((write(sock, p_frame, p_data - p_frame) < 0) &&
		//    !arg_ignore && (errno != EAGAIN)) {
		//	perror("sendto");
		//	exit(1);
		//}
		if (loops == 1)
			break;
		else if (loops)
			loops--;

		if (waittime > 0)
			usleep(waittime);
		if (arg_loops > 0) {
			volatile int loop = arg_loops;
			while (loop--);
		}
	}
    
	exit(0);
}