add netsimpcap, a network simulation device
[olsrd.git] / contrib / netsimpcap / src / netsimpcap.c
1 /*
2  * NetsimPcap - a userspace network bridge with simulated packet loss
3  *             Copyright 2008 H. Rogge (rogge@fgan.de)
4  * 
5  * This program is free software: you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation, either version 3 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
17  */
18
19 #include <errno.h>
20 #include <fcntl.h>
21 #include <pcap.h>
22 #include <pthread.h>
23 #include <stddef.h>
24 #include <stdio.h>
25 #include <string.h>
26 #include <stdarg.h>
27 #include <stdlib.h>
28 #include <signal.h>
29 #include <time.h>
30 #include <unistd.h>
31 #include <arpa/inet.h>
32 #include <linux/if_ether.h>
33 #include <linux/if_packet.h>
34 #include <linux/if_tun.h>
35 #include <net/if_arp.h>
36 #include <netinet/ip.h>
37 #include <netinet/udp.h>
38 #include <net/if.h>
39 #include <net/route.h>
40 #include <sys/ioctl.h>
41 #include <sys/time.h>
42 #include <sys/types.h>
43
44 #include "config.h"
45 #include "network_tap.h"
46
47 int debugMode = 0;
48
49 int running;
50
51 pcap_t* devices[128];
52 int deviceFD[128];
53 int deviceCount;
54
55 int tapFD;
56
57 u_int32_t *connBC;
58 u_int32_t *connUni;
59
60 u_int8_t buffer[65536];
61 u_int8_t mac_bc[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
62
63 /*
64  * signalHandler
65  * 
66  * This handler stops the mainloop when the programm is send a signal
67  * to stop
68  */
69 static void
70 signalHandler (int signo __attribute__((unused)))
71 {
72   running = 0;
73 }
74
75 /*
76  * calculateConnections
77  * 
78  * @param pointer to user defined connection matrix
79  * @param number of elements in a matrix line
80  * @param 1 if matrix contains drop-propabilities
81  */
82 void
83 calculateConnections (float *connMatrix, int socketcount, int drop)
84 {
85
86   /* calculating unicast/broadcast matrix */
87   connUni = malloc(sizeof(u_int32_t) * socketcount * socketcount);
88   memset(connUni, 0, sizeof(u_int32_t) * socketcount * socketcount);
89   connBC = malloc(sizeof(u_int32_t) * socketcount * socketcount);
90   memset(connBC, 0, sizeof(u_int32_t) * socketcount * socketcount);
91   
92   float broadcast, unicast;
93   int i, j;
94   for (j=0; j<socketcount; j++) {
95     for (i=0; i<socketcount; i++) {
96       float prop = connMatrix[GRID(i, j, socketcount)];
97       if (drop) {
98         prop = 1.0 - prop;
99       }
100       broadcast = prop;
101       
102       /* IEEE 802.11 do (normally) up to 4 retransmissions for unicast.
103        * A unicast package is only lost if all of this 5 transmissions
104        * are lost.
105        */      
106       prop = 1 - prop;
107       unicast = (1 - prop)*(1 + prop + prop*prop + prop*prop*prop + prop*prop*prop*prop);
108       
109       connBC[GRID(i, j, socketcount)] = (1<<24) * broadcast;
110       connUni[GRID(i, j, socketcount)] = (1<<24) * unicast;
111     }
112   }
113   
114   if (debugMode) {
115     printf("Connection matrix for unicast:\n");
116     for (j=0; j<socketcount; j++) {
117       for (i=0; i<socketcount; i++) {
118         if (i>0) {
119           printf(" ");
120         }
121         
122         printf("%1.2f", (float)connBC[GRID(i,j, socketcount)] / (float)(1<<24));
123       }
124       printf("\n");
125     }
126     printf("\nConnectionmatrix for broadcast:\n");
127     for (j=0; j<socketcount; j++) {
128       for (i=0; i<socketcount; i++) {
129         if (i>0) {
130           printf(" ");
131         }
132         
133         printf("%1.2f", (float)connUni[GRID(i,j, socketcount)] / (float)(1<<24));
134       }
135       printf("\n");
136     }
137   }
138 }
139
140 /*
141  * tap_callback
142  * 
143  * This function is called every times a package is received through the
144  * (optional) tap device. It retransmits the package to all connected devices.
145  */
146 void
147 tap_callback (void)
148 {
149   int len, i;
150   
151   len = read(tapFD, buffer, sizeof(buffer));
152   if (len > 0) {
153     for (i=0; i<deviceCount; i++) {
154       pcap_inject(devices[i], buffer, len);
155     }
156   }
157 }
158
159 /*
160  * capture_callback
161  * 
162  * This function is called for every package received through libpcap on
163  * one of the connected devices. It retransmit the package to the other
164  * devices (loss propability is defined in connection matrix) and always
165  * to the tap device (if active)
166  * 
167  * See libpcap documentation for parameters
168  */
169 void
170 capture_callback (u_char *args, const struct pcap_pkthdr* hdr, const u_char* packet)
171 {
172   int *index = (int *)args;
173   int unicast = memcmp(packet, mac_bc, 6) != 0;
174   
175   int i, len;
176   
177   len = hdr->len;
178   memcpy(buffer, packet, len);
179   
180   if (tapFD != -1) {
181     int send = 0;
182     while (send < len) {
183       int t = write(tapFD, &buffer[send], len - send);
184       if (t == -1) {
185         printf("Error while sending to tap device!\n");
186         break;
187       }
188       send += t;
189     }
190   }
191   
192   for (i=0; i<deviceCount; i++) {
193     u_int32_t prop;
194     if (unicast) {
195       prop = connUni[GRID(*index, i, deviceCount)];
196     } else {
197       prop = connBC[GRID(*index, i, deviceCount)];
198     }
199     
200     if (prop == 0 || prop < (rand() % (1<<24))) {
201       continue;
202     }
203     
204     pcap_inject(devices[i], buffer, len);
205   }
206 }
207
208 int
209 main (int argc, char **argv)
210 {
211   int i;
212   int dropPropability = 0;
213   char *connectionFile= NULL;
214   int deviceIndex = -1;
215   int hubMode = 0;
216   
217   MacAddress mac;
218   char *tapname = NULL;
219   
220   if (argc==1 || (argc == 2 && strcmp(argv[1], "--help")==0)) {
221     printf("%s: [-con <connectionfile>] [-hub] [-debug]"
222         " [-tap <devname> <mac>] [-drop] -dev <dev1> <dev2> <dev3>...\n", argv[0]);
223     return 0;
224   }
225   
226   deviceCount = 0;
227   tapFD = -1;
228   
229   for (i=1; i<argc; i++) {
230     if (strcmp(argv[i], "-con")==0 && i < argc-1) {
231       connectionFile = argv[i+1];
232       i++;
233     }
234     if (strcmp(argv[i], "-drop")==0) {
235       dropPropability = 1;
236     }
237     if (strcmp(argv[i], "-dev")==0) {
238       deviceIndex = ++i;
239       while (i < argc && argv[i][0] != '-') {
240         i++;
241         deviceCount++;
242       }
243       i--; /* to cancel the i++ in the for loop */
244     }
245     if (strcmp(argv[i], "-hub")==0) {
246       hubMode = 1;
247     }
248     if (strcmp(argv[i], "-tap")==0 && i < argc-2) {
249       tapname = argv[++i];
250       readMac(argv[++i], &mac);
251     }
252     if (strcmp(argv[i], "-debug")==0) {
253       debugMode = 1;
254     }
255   }
256   
257   if (hubMode == 1 && connectionFile != NULL) {
258     printf("Error, you cannot set matrix file in hub mode.\n");
259     return 1;
260   }
261   
262   if (connectionFile == NULL && hubMode == 0) {
263     printf("Error, netsim needs a matrix file for connections if not running in hub mode.\n");
264     return 1;
265   }
266   
267   if (deviceIndex < 0) {
268     printf("Error, you must specify the devices the programm connects to.\n");
269     return 1;
270   }
271   
272   if (deviceCount < 2) {
273     printf("Error, you need to bind at least two devices to the bridge.\n");
274     return 1;
275   }
276   
277   if (tapname) {
278     tapFD = createTap(tapname, &mac);
279     if (tapFD==-1) {
280       printf("Error, cannot open tap device '%s'\n", tapname);
281       return 1;
282     }
283     
284   }
285   running = 1;
286   
287   float *connMatrix = malloc(sizeof(float) * deviceCount * deviceCount);
288   if (!connMatrix) {
289     printf("Error, not enough memory for mac buffer!");
290     if (tapFD != -1)    closeTap(tapFD);
291     return 1;
292   }
293   
294   if (hubMode) {
295     int x,y;
296     for (y=0; y<deviceCount; y++) {
297       for (x=0; x<deviceCount; x++) {
298         if (x != y) {
299           connMatrix[GRID(x,y,deviceCount)] = 1.0;
300         }
301       }
302     }
303   } else {
304     if (readConnectionMatrix(connMatrix, connectionFile, deviceCount)) {
305       printf("Error while reading matrix file\n");
306       free(connMatrix);
307       if (tapFD != -1)    closeTap(tapFD);
308       return 1;
309     }
310   }
311   calculateConnections(connMatrix, deviceCount, dropPropability);
312   free(connMatrix);
313
314   char errbuf[PCAP_ERRBUF_SIZE];
315   int maxDeviceFD = 0;
316   
317   if (tapFD != -1) {
318     maxDeviceFD = tapFD;
319   }
320   for (i=0; i<deviceCount; i++) {
321     devices[i] = pcap_open_live(argv[i + deviceIndex], BUFSIZ, 0, -1, errbuf);
322     deviceFD[i] = -1;
323     if (devices[i] == NULL) {
324       printf("Error, cannot open pcap for device '%s'.\n", argv[i + deviceIndex]);
325       running = 0;
326     } else {
327       deviceFD[i] = pcap_fileno(devices[i]);
328       if (deviceFD[i] > maxDeviceFD) {
329         maxDeviceFD = deviceFD[i];
330       }
331     }
332   }
333   
334   /* activate signal handling */
335   signal(SIGABRT, &signalHandler);
336   signal(SIGTERM, &signalHandler);
337   signal(SIGQUIT, &signalHandler);
338   signal(SIGINT, &signalHandler);
339   
340   while (running) {
341     fd_set socketSet;
342     int socketsReady;
343     struct timeval timeout;
344     timeout.tv_sec = 1;
345     timeout.tv_usec = 0;
346     
347     FD_ZERO(&socketSet);
348     for (i=0; i<deviceCount; i++) {
349       FD_SET(deviceFD[i], &socketSet);
350     }
351     if (tapFD != -1) {
352       FD_SET(tapFD, &socketSet);
353     }
354     
355     socketsReady = select(maxDeviceFD+1, &socketSet, (fd_set *) 0, (fd_set *) 0, &timeout);
356     if (socketsReady <= 0) {
357       break;
358     }
359     
360     for (i=0; i<deviceCount; i++) {
361       if (FD_ISSET(deviceFD[i], &socketSet)) {
362         int error = pcap_dispatch(devices[i], -1, capture_callback, (u_char *)&i);
363         
364         if (error == -1) {
365           printf("Error during pcap_dispatch for device %s\n", argv[i + deviceIndex]);
366           running = 0;
367           break;
368         }
369       }
370     }
371     if (tapFD != -1 && FD_ISSET(tapFD, &socketSet)) {
372       tap_callback();
373     }
374   }
375   
376   for (i=0; i<deviceCount; i++) {
377     if (devices[i] != NULL) {
378       pcap_close(devices[i]);
379     }
380   }
381   free(connUni);
382   free(connBC);
383   
384   if (tapFD != -1)    closeTap(tapFD);
385   return 0;
386 }
387
388 /*
389  * Local Variables:
390  * c-basic-offset: 2
391  * End:
392  */