diff options
| -rw-r--r-- | pcap2tzsp.c | 153 |
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; } |