pcap_layer.cc
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           WMPSNIFFER          --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: pcap_layer.cc
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2012, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 #include <pcap.h>
00038 #include <cstdlib>
00039 #include <cstdio>
00040 #include <cstring>
00041 #include <fstream>
00042 #include <iostream>
00043 #include <sstream>
00044 #include <zlib.h>
00045 #include <ctime>
00046 #include <netinet/if_ether.h>
00047 #include <netinet/udp.h>
00048 #include <netinet/ip.h>
00049 #include "pcap_layer.h"
00050 #include "wmp_config.h"
00051 
00052 #include "core/include/frames.h"
00053 #include <stdlib.h>
00054 #include <iostream>
00055 #include "misc.h"
00056 
00057 static pcap_t *handle;
00058 static struct pcap_pkthdr header;
00059 static int offset;
00060 static bool pcap_keep_running;
00061 static int pfd[2];
00062 
00063 std::map<int, int> mac_ip;
00064 
00065 int pcap_init(char * dev, int num_nodes) {
00066         char errbuf[PCAP_ERRBUF_SIZE];
00067         fprintf(stderr, "Sniffing on %s\n", dev);
00068 
00069         /* Open the session in promiscuous mode */
00070 
00071         handle = pcap_open_live(dev, BUFSIZ, 1, 1000, errbuf);
00072 
00073         if (handle == NULL) {
00074                 fprintf(stderr, "Couldn't open device %s: %s\n", dev, errbuf);
00075                 exit(1);
00076         }
00077 
00078         pcap_keep_running = true;
00079         return 0;
00080 }
00081 
00082 double int2Rate(unsigned char val) {
00083         double rate;
00084         switch (val) {
00085         case 2:
00086                 rate = 1.0;
00087                 break;
00088         case 0xb:
00089                 rate = 5.5;
00090                 break;
00091         case 0x16:
00092                 rate = 11.0;
00093                 break;
00094         case 0x0c:
00095                 rate = 6.0;
00096                 break;
00097         case 0x04:
00098                 rate = 2.0;
00099                 break;
00100         case 0x12:
00101                 rate = 9.0;
00102                 break;
00103         default:
00104                 rate = val;
00105         }
00106         return rate;
00107 }
00108 
00109 int decompress(char * buf, int * size) {
00110         char tmp[2500];
00111         uLongf len = 2500;
00112         int res = uncompress((Bytef*) tmp, &len, (Bytef *) buf, *size);
00113         if (res != Z_OK) {
00114                 fprintf(stderr, "UNZIP ERROR!\n");
00115                 *size = -1;
00116                 return 0;
00117         }
00118         *size = len;
00119         memcpy(buf, tmp, len);
00120         return 1;
00121 }
00122 
00123 static int serial = 100;
00124 int pcap_sniff_packet(char * data, simData_Hdr & sd,
00125                 unsigned long long &time_us, std::map<int, robo_pose_t> & poses) {
00126         int dl = pcap_datalink(handle);
00127         char zFrame[2500];
00128         pcap_keep_running = true;
00129         while (pcap_keep_running) {
00130                 sd.frame_type = SP_FOREIGN;
00131                 sd.is_wmp = false;
00132                 int f_len = -1;
00133                 char * f_data = NULL;
00134                 int ds_status_offset = 0;
00135 
00136                 const u_char * packet = pcap_next(handle, &header);
00137                 if (packet == NULL) {
00138                         return -1;
00139                 }
00140 
00141                 int eth_hdr_offset = 0;
00142                 switch (dl) {
00143                 case 1:
00144                         offset = sizeof(ethhdr);
00145                         eth_hdr_offset = 0;
00146                         sd.rate = 0;
00147                         break;
00148 
00149                 case 127: {
00150                         ieee80211_radiotap_header * rth =
00151                                         (ieee80211_radiotap_header*) packet;
00152                         offset = rth->it_len + 24 + 8; //radiotap+80211
00153                         ds_status_offset = rth->it_len + 1;
00154                         sd.rate = int2Rate(packet[0x11]);
00155                         eth_hdr_offset = rth->it_len + 4;
00156                 }
00157                         break;
00158 
00159                 default:
00160                         offset = 0;
00161                         break;
00162                 };
00163 
00164                 const char * pkt_p = (char*) packet + offset;
00165 
00166                 if (header.len > offset) {
00167 
00168                         short protocol_type = ntohs(*(short*) (packet + offset - 2));
00169                         int mac_src = ((packet + eth_hdr_offset)[10] << 8)
00170                                         + ((packet + eth_hdr_offset)[11]);
00171                         int mac_dst = ((packet + eth_hdr_offset)[4] << 8)
00172                                         + ((packet + eth_hdr_offset)[5]);
00173                         struct udphdr * udph = (struct udphdr *) (pkt_p
00174                                         + sizeof(struct iphdr));
00175                         struct iphdr * iph = (struct iphdr *) pkt_p;
00176 
00177                         if (protocol_type == 0x0800 && iph->protocol == IPPROTO_UDP
00178                                         && htons(udph->dest) == 0x6969) {
00179                                 sd.is_wmp = true;
00180                                 if ((dl == 1
00181                                                 || (dl == 127 && (packet[ds_status_offset] & 3)) == 1)) {
00182                                         sd.frame_type = SP_LUS_WMP_FRAME;
00183                                 } else {
00184                                         sd.frame_type = SP_LUS_WMP_FRAME; //_DUP;
00185                                 }
00186                                 f_len = htons(udph->len) - sizeof(struct udphdr);
00187                                 f_data = (char *) (pkt_p + sizeof(struct udphdr)
00188                                                 + sizeof(struct iphdr));
00189                                 sd.is_wmp = true;
00190                                 sd.frame_type = SP_LUS_WMP_FRAME;
00191                         } else if (protocol_type == 0x0800 && iph->protocol == IPPROTO_UDP
00192                                         && htons(udph->dest) == 0x6868) {
00193                                 if (mac_ip.find(mac_src) == mac_ip.end()
00194                                                 || mac_ip.find(mac_dst) == mac_ip.end()) {
00195                                         continue;
00196                                 }
00197                                 f_data = (char *) (pkt_p + sizeof(struct udphdr)
00198                                                 + sizeof(struct iphdr));
00199                                 wmpFrame * p = (wmpFrame *) f_data;
00200 
00201                                 p->hdr.type = MESSAGE;
00202                                 p->hdr.from = mac_ip[mac_src];
00203                                 p->hdr.to = mac_ip[mac_dst];
00204 
00205                                 f_len =
00206                                                 header.len
00207                                                                 - (offset + sizeof(struct udphdr)
00208                                                                                 + sizeof(struct iphdr));
00209                                 sd.is_wmp = true;
00210                                 sd.frame_type = SP_LUS_WMP_FRAME;
00211                         } else if (protocol_type == 0x0800 && iph->protocol == IPPROTO_UDP) {
00212                                 if (mac_ip.find(mac_src) == mac_ip.end()
00213                                                 || mac_ip.find(mac_dst) == mac_ip.end()) {
00214                                         continue;
00215                                 }
00216 
00217                                 f_data = (char *) (pkt_p + sizeof(struct iphdr)
00218                                                 + sizeof(struct udphdr));
00219                                 wmpFrame * p = (wmpFrame *) f_data;
00220 
00221                                 p->hdr.from = mac_ip[mac_src];
00222                                 p->hdr.to = mac_ip[mac_dst];
00223                                 p->hdr.type = MESSAGE;
00224                                 p->msg.len =
00225                                                 header.len
00226                                                                 - (offset + sizeof(struct udphdr)
00227                                                                                 + sizeof(struct iphdr));
00228                                 f_len = htons(udph->len) - sizeof(struct udphdr);
00229 
00230                                 sd.is_wmp = true;
00231                                 sd.frame_type = SP_LUS_WMP_FRAME;
00232 
00233                         } else if (protocol_type == 0x0800) {
00234                                 if (mac_ip.find(mac_src) == mac_ip.end()
00235                                                 || mac_ip.find(mac_dst) == mac_ip.end()) {
00236                                         continue;
00237                                 }
00238                                 f_data = (char *) (pkt_p + sizeof(struct iphdr));
00239                                 wmpFrame * p = (wmpFrame *) f_data;
00240 
00241                                 p->hdr.from = mac_ip[mac_src];
00242                                 p->hdr.to = mac_ip[mac_dst];
00243 
00244                                 p->hdr.type = MESSAGE;
00245                                 p->hdr.serial = serial++;
00246                                 p->msg.len = header.len - (offset + sizeof(struct iphdr));
00247                                 p->msg.src = p->hdr.from;
00248                                 p->msg.dest = p->hdr.to;
00249                                 p->hdr.retries = 0;
00250                                 p->msg.msg_hash = serial;
00251 
00252                                 f_len = header.len - (offset + sizeof(struct iphdr));
00253                                 if (f_len < sizeof(wmpFrame)) {
00254                                         f_len = sizeof(wmpFrame);
00255                                 }
00256                                 sd.is_wmp = true;
00257                                 sd.frame_type = SP_LUS_WMP_FRAME;
00258 
00259                         } else if (protocol_type == 0x6969 || protocol_type == 0x6970
00260                                         || protocol_type == 0x6971) {
00261 
00262                                 f_data = (char*) &packet[offset];
00263                                 f_len = header.len - offset;
00264                                 sd.proto = protocol_type;
00265                                 if (protocol_type == 0x6970) {
00266 
00267                                         memcpy(zFrame, f_data, f_len);
00268                                         decompress(zFrame, &f_len);
00269                                         f_data = zFrame;
00270 
00271                                 } else if (protocol_type == 0x6971) {
00272                                         memcpy(zFrame, f_data, f_len);
00273                                         f_data = zFrame;
00274                                 }
00275                                 sd.frame_type = SP_LUS_WMP_FRAME;
00276                                 sd.is_wmp = true;
00277                         } else {
00278                                 sd.is_wmp = false;
00279                         }
00280                 } else {
00281                         sd.is_wmp = false;
00282                 }
00283                 time_us = header.ts.tv_sec;
00284                 time_us = time_us * 1000000 + header.ts.tv_usec;
00285                 sd.time = time_us;
00286                 sd.data_src = 32;
00287 
00288                 if (sd.is_wmp) {
00289                         wmpFrame * p = (wmpFrame*) f_data;
00290                         memcpy(data, f_data, f_len);
00291                         poses[p->hdr.from].reached = true;
00292                         poses[p->hdr.from].pose_is_valid = false;
00293                         return f_len;
00294                 } else {
00295                         return header.len;
00296                 }
00297         }
00298         return 0;
00299 }
00300 
00301 void pcap_layer_close() {
00302         pcap_keep_running = false;
00303 }


ros_rt_wmp_sniffer
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:57