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