Première version du client TZSP

git-svn-id: file:///var/svn/2012-tzsp/trunk@2 147d2d3d-d0bd-48ea-923a-d90ac20f5906

3 files changed, 397 insertions, 0 deletions

diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..c9d7637
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,8 @@
+all : pcap2tzsp
+
+pcap2tzsp: pcap2tzsp.o
+	gcc -o pcap2tzsp pcap2tzsp.o -lpcap 
+pcap2tzsp.o : pcap2tzsp.c
+	gcc -Wall -c pcap2tzsp.c
+clean :
+	@rm pcap2tzsp.o pcap2tzsp
diff --git a/pcap2tzsp.c b/pcap2tzsp.c
new file mode 100644
index 0000000..294ec32
--- /dev/null
+++ b/pcap2tzsp.c
@@ -0,0 +1,304 @@
+
+/* Basics */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+/* Args parsing and basename() for usage */
+#include <getopt.h>
+#include <libgen.h>
+
+/* UDP stuff */
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+/* Packet capture stuff */
+#include <pcap.h>
+#include <sys/time.h>
+
+#define DEFAULT_FILTER "not ( udp and dst %s and port %s )"
+#define DEFAULT_HOST "127.0.0.1"
+#define DEFAULT_PORT "37008"
+#define DEFAULT_SNAPLEN "64"
+
+#define UDP_SEND_BUFLEN 1500
+#define MAX_TZSP_PAYLOAD (1500-40-16)
+#define MAX_BYTES_TO_CAPTURE (1500-40-16)
+
+
+/*
+TODO List
+  * Corriger bug du filtre par défaut qui capture qd même le traffic TZSP émis
+  * Utiliser le opt_snaplen réellement
+  * Resolution DNS host -> IP (c'est l'ip qu'il faut dans le filtre et pas le host !!)
+  * Stats nombre de packets loupés ?
+  * Implémenter une bwlimit en sortie (ptks/s et/ou bytes/s)
+  * Pkt timestamps sur 64bits (quel field prendre ?)
+  * Comparer les headers envoyés par un mikrotik avec /tool sniffer
+  * free() de tous les malloc(), calloc() et strdup()
+  * Graceful stop avec signal()
+  * Licence GPL
+*/
+
+/* Functions declaration */
+void capture_loop(char pcap_filter[]); 
+void process_packet(u_char *void_args, const struct pcap_pkthdr* pkthdr, const u_char * packet);
+
+/* Custom types definition */
+typedef struct _process_packet_args {
+	uint64_t pkt_count;
+	int udp_socket;
+	struct sockaddr_in udp_sockaddr;
+} process_packet_args_t;
+
+/* Constants */
+const int sockaddr_len=sizeof(struct sockaddr_in);
+
+
+/* Flags from commandline parsing */
+static int opt_verbose;
+/* Option arguments from commandline parsing */
+static char *opt_iface=NULL;
+static char *opt_host=NULL;
+static char *opt_port=NULL;
+static char *opt_snaplen=NULL;
+
+
+void usage(char progname[]) {
+	printf("Usage : %s [--verbose] [--brief] [-i <iface>] [-h <host>] [-p <port>] [custom_pcap_filter]\n", progname);
+	printf("\t<iface> : Interface name to capture from (Default : first available interface)\n");
+	printf("\t<host> : Host (or IPv4 address) for sending captured packet headers (Default : '%s')\n", DEFAULT_HOST);
+	printf("\t<port> : Port for sending captured packet headers (Default '%s')\n", DEFAULT_PORT);
+	printf("\t<custom_pcap_filter> : libpcap capture filter (Default '%s')\n", DEFAULT_FILTER);
+	exit(1);
+}
+
+int main(int argc, char *argv[]) {
+
+	/* Command line args parsing */
+	int c,i,j,pcap_filter_len;
+	char *pcap_filter=NULL;
+
+	while (1) {
+		static struct option long_options[] = {
+			/* These options set a flag. */
+			{"verbose", no_argument, &opt_verbose, 1},
+			{"brief",   no_argument, &opt_verbose, 0},
+			/* These options don't set a flag.
+			   We distinguish them by their indices. */
+			{"interface", required_argument, 0, 'i'},
+			{"host",      required_argument, 0, 'h'},
+			{"port",      required_argument, 0, 'p'},
+			{"snaplen",   required_argument, 0, 's'},
+			{0, 0, 0, 0}
+		};
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "i:h:p:s:", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1) break;
+
+		switch (c) {
+			case 0:
+				/* If this option set a flag, do nothing else now. */
+				if (long_options[option_index].flag != 0) break;
+
+				//printf ("option %s", long_options[option_index].name);
+				//if (optarg) printf (" with arg %s", optarg);
+				//printf ("\n");
+
+				break;
+			case 'i': opt_iface=	strdup(optarg); break;
+			case 'h': opt_host=	strdup(optarg); break;
+			case 'p': opt_port=	strdup(optarg); break;
+			case 's': opt_snaplen=	strdup(optarg); break;
+			case '?':
+				/* getopt_long already printed an error message. */
+				usage(basename(argv[0]));
+				break;
+			default:
+				abort();
+		}
+	}
+
+	// Assign default value if no user supplied value
+	if (opt_host==NULL)	opt_host=strdup(DEFAULT_HOST);
+	if (opt_port==NULL)	opt_port=strdup(DEFAULT_PORT);
+	if (opt_snaplen==NULL)	opt_snaplen=strdup(DEFAULT_SNAPLEN);
+	// No default for opt_iface because will be choosen at runtime
+
+	//if (verbose_flag) puts ("verbose flag is set");
+
+	/* Construct the pcap_filter */
+	pcap_filter_len=0;
+	if (optind < argc) {
+		// Any remaining command line arguments is for pcap_filter
+		for (i=optind; i<argc; i++) pcap_filter_len += strlen(argv[i])+1;
+
+		pcap_filter=calloc(pcap_filter_len+1, sizeof(char));
+		if (pcap_filter==NULL) {
+			fprintf(stderr,"calloc() failed for pcap_filter\n");
+			abort();
+		}
+
+		for (i=optind, j=0; i<argc; i++) {
+			if (i<argc-1) {
+				 j+=sprintf(pcap_filter+j, "%s ", argv[i]);
+			} else {
+				j+=sprintf(pcap_filter+j, "%s", argv[i]);
+			}
+		}
+	} else {
+		// Else, use the default filter (skip the TZSP packet flow to host:port)
+		pcap_filter_len = strlen(DEFAULT_FILTER) -4 + strlen(opt_host) + strlen(opt_port) + 1;
+		pcap_filter=calloc(pcap_filter_len+1, sizeof(char));
+		j=sprintf(pcap_filter, DEFAULT_FILTER, opt_host, opt_port);
+	}
+	//printf ("Capture filter will be : '%s' (pcap_filter_len-j==%i)\n", pcap_filter, pcap_filter_len-j);
+
+
+	/* Run the capture loop */
+	capture_loop(pcap_filter);
+
+	return 0;
+}
+
+
+void capture_loop(char pcap_filter[]) { 
+
+	pcap_t *pcap_handle = NULL; 
+	static char *pcap_device=NULL;
+	char pcap_errbuf[PCAP_ERRBUF_SIZE];
+	struct bpf_program pcap_filter_prog;
+	process_packet_args_t process_packet_args;
+
+	memset(pcap_errbuf,0,PCAP_ERRBUF_SIZE); 
+	memset(&process_packet_args,0,sizeof(process_packet_args_t)); 
+
+	if (opt_iface == NULL) {
+		/* Get the name of the first device suitable for capture */ 
+		if ( (pcap_device = pcap_lookupdev(pcap_errbuf)) == NULL){
+			fprintf(stderr, "ERROR: %s\n", pcap_errbuf);
+			exit(20);
+		}
+	} else {
+		/* Use user-suplied interface */
+		pcap_device=strdup(opt_iface);
+	}
+
+	//Already done : memset((char *) &(process_packet_args.udp_sockaddr), 0, sockaddr_len);
+	process_packet_args.udp_sockaddr.sin_family = AF_INET;
+	process_packet_args.udp_sockaddr.sin_port = htons(atoi(opt_port));
+	if (inet_aton(opt_host, &(process_packet_args.udp_sockaddr.sin_addr))==0) {
+		fprintf(stderr, "inet_aton : Impossible de parser l'IP %s\n",opt_host);
+		exit(11);
+	}
+	
+
+	/* Initialisation pcap */
+	if (opt_verbose) printf("Opening device %s for capturing\n", pcap_device);
+
+	/* Open device in promiscuous mode */ 
+	if ( (pcap_handle = pcap_open_live(pcap_device, MAX_BYTES_TO_CAPTURE, 1,  512, pcap_errbuf)) == NULL){
+		fprintf(stderr, "ERROR: %s\n", pcap_errbuf);
+		exit(21);
+	}
+
+	if (opt_verbose) printf("Compiling the following capture filter : '%s'\n", pcap_filter);
+	if ( pcap_compile(pcap_handle,&pcap_filter_prog,pcap_filter,1,PCAP_NETMASK_UNKNOWN) == -1 ) {
+		fprintf(stderr,"ERROR : %s\n", pcap_geterr(pcap_handle) );
+		exit(22);
+	}
+
+	if ( pcap_setfilter(pcap_handle,&pcap_filter_prog) == -1 ) {
+		fprintf(stderr,"ERROR : %s\n", pcap_geterr(pcap_handle) );
+		exit(23);
+	}
+
+	/* Initialisation socket UDP */
+	process_packet_args.udp_socket=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (process_packet_args.udp_socket==-1) {
+		perror("socket");
+		exit(10);
+	}
+
+	/* Loop forever & call process_packet() for every received packet*/ 
+	if ( pcap_loop(pcap_handle, -1, process_packet, (u_char *)&process_packet_args) == -1){
+		fprintf(stderr, "ERROR: %s\n", pcap_geterr(pcap_handle) );
+		exit(25);
+	}
+
+	close(process_packet_args.udp_socket);
+}
+
+
+/* process_packet(): Callback function called by pcap_loop() everytime a packet */
+/* arrives to the network card. This function prints the captured raw data in  */
+/* hexadecimal.                                                                */
+void process_packet(u_char *void_args, const struct pcap_pkthdr* pkthdr, const u_char * packet) {
+
+	static time_t old_tv_sec=0;
+	static uint64_t old_pkt_count=0;
+
+	int res;
+	double throughput;
+
+	uint32_t ts; /* In network byte order */
+	uint16_t len; /* In network byte order */
+	char buf[UDP_SEND_BUFLEN];
+
+	process_packet_args_t *args=(process_packet_args_t *)void_args; 
+
+	/* Packet counting and displaying (max once by second) */
+	args->pkt_count++;
+
+	if (old_tv_sec != pkthdr->ts.tv_sec) {
+		//printf("DEBUG : throughput=((double)  %llu - %llu ) / (  %u - %u )\n", args->pkt_count, old_pkt_count, pkthdr->ts.tv_sec, old_tv_sec);
+		throughput=((double) args->pkt_count - old_pkt_count ) / (pkthdr->ts.tv_sec - old_tv_sec);
+		printf("\rPacket Count: %20llu (%8.1f pkt/s)", args->pkt_count, throughput);
+		fflush(stdout);
+		old_tv_sec=pkthdr->ts.tv_sec;
+		old_pkt_count= args->pkt_count;
+	}
+
+	/* Variable fields for TZSP packet */
+	ts=htonl((uint32_t) pkthdr->ts.tv_sec); /* TODO : this cast is bullshit ? */
+	len=htons((uint16_t) pkthdr->len);
+
+	/* TaZmen Sniffing Protocol (TZSP) packet forging */
+
+	//memset((char *) &buf, 0, UDP_SEND_BUFLEN); /* Buffer reset not useful for now */
+	buf[0]=0x01; /* Version */
+	buf[1]=0x01; /* Type == Packet for transmit */
+	buf[2]=0x00; /* Encapsuled protocol (2 bytes) 0x0001 == Ethernet */
+	buf[3]=0x01;
+
+	buf[4]=0x0D; /* Tag type == TAG_TIMESTAMP */
+	buf[5]=0x04; /* Tag length == 4 bytes */
+
+	/* buf[6,7,8,9] Timestamp on 4 bytes (network order) */
+	memcpy(buf+6, &ts, 4);	
+
+	buf[10]=0x29; /* Tag type TAG_RX_FRAME_LENGTH */
+	buf[11]=0x02; /* Tag length : 2 bytes */
+	memcpy(buf+12,&len, 2);
+
+	buf[14]=0x00; /* Tag type TAG PADDING (for alignement) */
+	buf[15]=0x01; /* Tag type TAG END */
+
+	/* Raw packet copy */
+	//TODO : assert that pkthdr->caplen < MAX_TZSP_PAYLOAD
+	memcpy(buf+16,packet, pkthdr->caplen);
+
+	/* TZSP packet sending (over the UDP socket) */
+	res=sendto(args->udp_socket, buf, 16+pkthdr->caplen, 0, (struct sockaddr *)&(args->udp_sockaddr), sockaddr_len);
+	if (res==-1) {
+		fprintf(stderr, "sendto() error\n");
+	}
+}
+
diff --git a/tests.c b/tests.c
new file mode 100644
index 0000000..dd6563e
--- /dev/null
+++ b/tests.c
@@ -0,0 +1,85 @@
+
+
+void diep(char s[], int r) {
+	perror(s);
+	exit(r);
+}
+
+void die(char s[], int r) {
+	fprintf(stderr, "%s() failed\n", s);
+	exit(r);
+}
+
+#define DEST_IP "127.0.0.1"
+#define DEST_PORT 37008
+int send_tzsp_sample_packet() {
+	const int sockaddr_len=sizeof(struct sockaddr_in);
+	const char sample_packet[]={
+	/*Ether*/ 0xe0,0x91,0xf5,0xa1,0xa3,0xd8,0x00,0x11,0x2f,0xde,0x0d,0xa1,0x08,0x00,
+	/* IP4 */ 0x45,0x00,0x00,0x3c,0x00,0x00,0x40,0x00,0x40,0x11,0xb4,0x8d,0xc0,0xa8,0x02,0xd2,0xc0,0xa8,0x02,0x01,
+	/* UDP */ 0xc4,0x89,0x00,0x35,0x00,0x28,0x86,0x5d,
+	/* DNS */ 0x04,0x59,0x01,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x77,0x77,0x77,
+		  0x06,0x64,0x6f,0x6f,0x64,0x6c,0x65,0x03,0x63,0x6f,0x6d,0x00,0x00,0x01,0x00,0x01
+	};
+	const int sample_packet_len=sizeof(sample_packet);
+	struct timeval sample_packet_time;
+	uint32_t ts;
+	
+	int res,len;
+	int s; /*socket*/
+	struct sockaddr_in /*si_me,*/ si_other;
+	char buf[UDP_SEND_BUFLEN];
+
+	/* On récupère le timestamp courant pour mettre dans les headers TZSP du paquet capturé d'exemple */
+	printf("DEBUG : sizeof(time_t)==%i\n", sizeof(time_t));
+	res=gettimeofday(&sample_packet_time,NULL);
+	if (res==-1) die("gettimeofday",5);
+
+
+	/* On prépare le socket UDP */
+	s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (s==-1) diep("socket",10);
+
+	memset((char *) &si_other, 0, sockaddr_len);
+	si_other.sin_family = AF_INET;
+	si_other.sin_port = htons(DEST_PORT);
+	res=inet_aton(DEST_IP, &si_other.sin_addr);
+	if (res==0) die("inet_aton",11);
+
+	/* On forge la trame TZSP */
+
+	//memset((char *) &buf, 0, UDP_SEND_BUFLEN);
+	buf[0]=0x01; /* version */
+	buf[1]=0x01; /* type == Packet for transmit */
+	buf[2]=0x00; /* Encapsuled protocol 0x0001 == Ethernet */
+	buf[3]=0x01;
+
+	buf[4]=0x0D; /* Tag type TAG_TIMESTAMP */
+	buf[5]=0x04; /* Tag length : 4 bytes */
+
+	/* buf[6,7,8,9] Timestamp on 4 bytes (network order) */
+	ts=htonl((uint32_t) sample_packet_time.tv_sec); /* FIXME : ce cast est une bonne idée ??? */
+	memcpy(buf+6, &ts, sizeof(ts));	
+
+	buf[10]=0x29; /* Tag type TAG_RX_FRAME_LENGTH */
+	buf[11]=0x02; /* Tag length : 2 bytes */
+	len=htons((uint16_t) sample_packet_len);
+	memcpy(buf+12,&len, sizeof(len));
+
+	buf[14]=0x00; /* Tag type TAG PADDING (for alignement) */
+	buf[15]=0x01; /* Tag type TAG END */
+
+	/* Raw packet copy */
+	//TODO : assert that sample_packet_len < MAX_TZSP_PAYLOAD
+	memcpy(buf+16,sample_packet, sample_packet_len);
+
+	/* Envoi du paquet UDP (avec comme charge utile le TZSP) */
+	res=sendto(s, buf, 16+sample_packet_len, 0, (struct sockaddr *)&si_other, sockaddr_len);
+	if (res==-1) diep("sendto",12);
+
+	close(s);
+
+	return 0;
+}
+
+