Work in progress : ajout de fonctions pour la génération des paquets TZSP.

Plusieurs possibilités vont être proposées (plus ou moins de tags).
make_tzsp_basic testé sous Linux. Compil sous Windows à vérifier (size_t et compagnie).


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

1 files changed, 105 insertions, 48 deletions

diff --git a/pcap2tzsp.c b/pcap2tzsp.c
index 16b6e56..656e84a 100644
--- a/pcap2tzsp.c
+++ b/pcap2tzsp.c
@@ -55,6 +55,9 @@ TODO List
 SOCKET make_dgram_socket(char host[], char service[], int hint_flags, char **resolved_address);
 void start_capture_loop(char *pcap_filter);
 void process_packet(u_char *void_args, const struct pcap_pkthdr* pkthdr, const u_char * packet);
+void make_tzsp_notags(u_char *out_tzsp, size_t *tzsplen, const u_char *pdu, size_t pdu_len);
+void make_tzsp_basic(u_char *out_tzsp, size_t *tzsplen, const u_char *pdu, size_t pdu_len, time_t original_packet_timestamp, size_t original_packet_len );
+
 
 /* Custom types definition */
 typedef struct _process_packet_args {
@@ -299,7 +302,7 @@ void start_capture_loop(char *pcap_filter) {
 	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);
+			fprintf(stderr, "ERROR: %s (maybe you forget start this as root)\n", pcap_errbuf);
 			exit(20);
 		}
 	} else {
@@ -389,13 +392,11 @@ void process_packet(u_char *void_args, const struct pcap_pkthdr* pkthdr, const u
 	static time_t old_tv_sec=0;
 	static u_int old_captured_pkt_count=0;
 
-	int res, tzsp_len;
+	int res;
+	size_t tzsp_len;
 	double throughput;
 
-	/*uint32_t pkt_num; // In network byte order */
-	uint32_t field_ts; /* In network byte order */
-	uint16_t field_len; /* In network byte order */
-	char buf[UDP_SEND_BUFLEN];
+	u_char buf[UDP_SEND_BUFLEN];
 	/* struct timespec ts_reqsleep; // For simulation */
 
 	process_packet_args_t *args=(process_packet_args_t *)void_args;
@@ -416,58 +417,114 @@ void process_packet(u_char *void_args, const struct pcap_pkthdr* pkthdr, const u
 
 	//TODO : bwlimit HERE (not before, not after)
 
-	/* Variable fields for TZSP packet */
-	field_ts=htonl((uint32_t) pkthdr->ts.tv_sec); /* TODO : this cast is bullshit ? */
-	field_len=htons((uint16_t) pkthdr->len);
-	//pkt_num=htons((uint32_t) args->captured_pkt_count);
-
 	/* TaZmen Sniffing Protocol (TZSP) packet forging */
+	tzsp_len=UDP_SEND_BUFLEN;
+	switch(0) { /* TODO */
+		case 1:
+			make_tzsp_notags(buf, &tzsp_len,packet,pkthdr->caplen);
+			break;
+		default:			
+			make_tzsp_basic(buf, &tzsp_len,packet,pkthdr->caplen, pkthdr->ts.tv_sec, pkthdr->len);
+	}
+	if (tzsp_len < 0) {
+		fprintf(stderr, "make_tzsp_*() problem\n");
+		return;
+	}
 
-	//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;
+	/* TZSP packet sending (over the UDP socket) */
 
-	buf[4]=0x0D; /* Tag type == TAG_TIMESTAMP */
-	buf[5]=0x04; /* Tag length == 4 bytes */
+	/* Slow sending simulation
+	ts_reqsleep.tv_sec=0;
+	ts_reqsleep.tv_nsec=10000;
+	nanosleep(&ts_reqsleep, NULL);
+	*/
+#ifdef WIN32
+	res=send(args->socket, buf, tzsp_len, 0);
+#else
+	res=write(args->socket, buf, tzsp_len);
+#endif
+	if (res != tzsp_len) {
+		fprintf(stderr, "write() on UDP socket error (written %i of %zu bytes)\n", res, tzsp_len);
+		return;
+	}
+
+	args->sent_pkt_count++;
+}
 
-	/* buf[6,7,8,9] Timestamp on 4 bytes (network order) */
-	memcpy(buf+6, &field_ts, 4);
+void make_tzsp_notags(u_char *out_tzsp, size_t *tzsplen, const u_char *pdu, size_t pdu_len) {
+	const char tzsp_header_tpl[] = {
+		0x01,		/* Version 1 */
+		0x01,		/* Type : Packet for transmit */
+		0x00, 0x01,	/* Encapsulated protocol : 0x001 == Ethernet */
+		0x01		/* Tag type : TAG END */
+	};
+	const size_t tzsp_header_len = sizeof(tzsp_header_tpl);
+	size_t needed = tzsp_header_len + pdu_len;
+
+	/* Check if enought space in out buffer */
+	if ( *tzsplen < needed ) {
+		*tzsplen=-1;
+		return;
+	}
 
-	/* Wireshark don't dissect that */
-	//buf[10]=0x28; /* Tag type TAG_PACKET_COUNT */
-	//buf[11]=0x04; /* Tag length == 4 bytes */
-	//memcpy(buf+12,&pkt_num, 4);
+	/* Header copy */
+	memcpy(out_tzsp, tzsp_header_tpl, tzsp_header_len);
 
-	buf[10]=0x29; /* Tag type TAG_RX_FRAME_LENGTH */
-	buf[11]=0x02; /* Tag length == 2 bytes */
-	memcpy(buf+12, &field_len, 2);
+        /* Raw packet copy */
+        memcpy(out_tzsp + tzsp_header_len, pdu, pdu_len);
 
-	buf[14]=0x00; /* Tag type TAG PADDING (for alignement) */
-	buf[15]=0x01; /* Tag type TAG END */
+	/* Update tzsplen to the effecive len */
+	*tzsplen=needed;
 
-	/* Raw packet copy */
-	//TODO : assert that pkthdr->caplen < MAX_TZSP_PAYLOAD
-	memcpy(buf+16,packet, pkthdr->caplen);
+	return;
+}
 
-	/* TZSP packet sending (over the UDP socket) */
-	tzsp_len=16+pkthdr->caplen;
-#ifdef WIN32
-	res=send(args->socket, buf, tzsp_len,0);
-#else
-	res=write(args->socket, buf, tzsp_len);
-#endif
-	if (res != tzsp_len) {
-		fprintf(stderr, "write() on UDP socket error (written %i of %i bytes)\n", res, tzsp_len);
-	} else {
-		args->sent_pkt_count++;
+void make_tzsp_basic(u_char *out_tzsp, size_t *tzsplen, const u_char *pdu, size_t pdu_len, time_t original_packet_timestamp, size_t original_packet_len ) {
+	const char tzsp_header_tpl[] = {
+		0x01,		/* Version 1 */
+		0x01,		/* Type : 0x01 == Packet for transmit */
+		0x00, 0x01,	/* Encapsulated protocol : 0x0001 == Ethernet */
+
+		0x0D,		/* Tag type TAG_TIMESTAMP */
+		0x04,		/* Tag len : 4 bytes */
+		0x00, 0x00, 0x00, 0x00, /* Tag Value (placeholder) */
+
+		0x29,		/* Tag type TAG_RX_FRAME_LENGTH */
+		0x02,		/* Tag len : 2 bytes */
+		0x00, 0x00, 	/* Tag Value (placeholder) */
+
+		0x00,		/* Tag type : TAG PADDING (for alignement) */
+		0x01		/* Tag type : TAG END */
+	};
+	const size_t tzsp_header_len = sizeof(tzsp_header_tpl);
+	uint32_t tag_timestamp_value;
+	uint16_t tag_packet_len_value;
+	size_t needed;
+
+	/* Check if enough space in out buffer */
+	needed = tzsp_header_len + pdu_len;
+	if ( *tzsplen < needed ) {
+		*tzsplen=-1;
+		return;
 	}
 
-	/* Slow sending simulation
-	ts_reqsleep.tv_sec=0;
-	ts_reqsleep.tv_nsec=10000;
-	nanosleep(&ts_reqsleep, NULL);
-	*/
+	/* "Compute" tags values (network byte order) */
+	tag_timestamp_value=htonl((uint32_t) original_packet_timestamp);
+	tag_packet_len_value=htons((uint16_t) original_packet_len);
+
+	/* Header template copy */
+	memcpy(out_tzsp, tzsp_header_tpl, tzsp_header_len);
+
+	/* Tag values copy */
+	memcpy(out_tzsp+6, &tag_timestamp_value, sizeof(uint32_t));
+	memcpy(out_tzsp+12, &tag_packet_len_value, sizeof(uint16_t));
+
+        /* Raw packet copy */
+        memcpy(out_tzsp + tzsp_header_len, pdu, pdu_len);
+
+	/* Update tzsplen to the effecive len */
+	*tzsplen=needed;
+
+	return;
 }