Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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;
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;
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 }