7cefe69e80352865e53e1b5fa9b929263d8b1905
[olsrd.git] / lib / bmf / src / Bmf.c
1 /*
2  * OLSR Basic Multicast Forwarding (BMF) plugin.
3  * Copyright (c) 2005, 2006, Thales Communications, Huizen, The Netherlands.
4  * Written by Erik Tromp.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without 
8  * modification, are permitted provided that the following conditions 
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright 
12  *   notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright 
14  *   notice, this list of conditions and the following disclaimer in 
15  *   the documentation and/or other materials provided with the 
16  *   distribution.
17  * * Neither the name of Thales, BMF nor the names of its 
18  *   contributors may be used to endorse or promote products derived 
19  *   from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
22  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
23  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
24  * IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
25  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 
27  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
28  * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
29  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 
30  * OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32
33 /* $Id: Bmf.c,v 1.1 2006/05/03 08:59:04 kattemat Exp $ */
34
35 #define _MULTI_THREADED
36
37 #include "Bmf.h"
38
39 /* System includes */
40 #include <stdio.h> /* NULL */
41 #include <sys/types.h> /* ssize_t */
42 #include <string.h> /* strerror() */
43 #include <errno.h> /* errno */
44 #include <assert.h> /* assert() */
45 #include <linux/if_packet.h> /* struct sockaddr_ll, PACKET_MULTICAST */
46 #include <pthread.h> /* pthread_create() */
47 #include <signal.h> /* SIGINT */
48
49 /* OLSRD includes */
50 #include "defs.h" /* olsr_cnf */
51 #include "olsr.h" /* olsr_printf */
52 #include "scheduler.h" /* olsr_register_scheduler_event */
53 #include "mid_set.h" /* mid_lookup_main_addr() */
54 #include "mpr_selector_set.h" /* olsr_lookup_mprs_set() */
55
56 /* Plugin includes */
57 #include "NetworkInterfaces.h" /* TBmfInterface, CreateBmfNetworkInterfaces(), CloseBmfNetworkInterfaces() */
58 #include "Address.h" /* IsMulticast(), IsLocalBroadcast() */
59 #include "Packet.h" /* ETH_TYPE_OFFSET, IFHWADDRLEN etc. */
60 #include "PacketHistory.h" /* InitPacketHistory() */
61 #include "DropList.h" /* DropMac() */
62
63 static pthread_t BmfThread;
64 static int BmfThreadRunning = 0;
65
66
67 static void BmfPacketCaptured(struct TBmfInterface* intf, unsigned char* buffer, ssize_t len)
68 {
69   struct interface* ifFrom;
70   unsigned char* srcMac;
71   union olsr_ip_addr srcIp;
72   union olsr_ip_addr destIp;
73   union olsr_ip_addr* originator;
74   struct sockaddr_in sin;
75   struct TBmfInterface* nextFwIntf;
76
77   /* Only forward IPv4 packets */
78   u_int16_t type;
79   memcpy(&type, buffer + ETH_TYPE_OFFSET, 2);
80   if (ntohs(type) != IPV4_TYPE)
81   {
82     return;
83   }
84
85   /* Lookup the OLSR interface on which this packet is received */
86   ifFrom = intf->olsrIntf;
87
88   /* Only forward multicast or local broadcast packets */
89   COPY_IP(&destIp, buffer + IP_HDR_OFFSET + IPV4_OFFSET_DSTIP);
90   if (! IsMulticast(&destIp) && ! IsLocalBroadcast(&destIp, ifFrom))
91   {
92     return;
93   }
94   
95   /* Discard OLSR packets (UDP port 698) and BMF encapsulated packets
96    * (UDP port 50505) */
97   if (IsOlsrOrBmfPacket(buffer, len))
98   {
99     return;
100   }
101
102   /* Apply drop list for testing purposes. */
103   srcMac = buffer + IFHWADDRLEN;
104   if (IsInDropList(srcMac))
105   {
106     return;
107   }
108
109   /* Lookup main address of source */
110         COPY_IP(&srcIp, buffer + IP_HDR_OFFSET + IPV4_OFFSET_SRCIP);
111   originator = mid_lookup_main_addr(&srcIp);
112   if (originator == NULL)
113   {
114     originator = &srcIp;
115   }
116
117   olsr_printf(
118     9,
119     "MC pkt to %s received from originator %s (%s) via \"%s\"\n",
120     olsr_ip_to_string(&destIp),
121     olsr_ip_to_string(originator),
122     olsr_ip_to_string(&srcIp),
123     intf->ifName);
124
125   /* Check if I am MPR for that originator */
126   if (ifFrom != NULL && olsr_lookup_mprs_set(originator) == NULL)
127   {
128     olsr_printf(
129       9,
130       "--> Discarding pkt: I am not selected as MPR by that originator\n");
131     return;
132   }
133
134   /* If this packet is captured on a non-OLSR interface, decrease
135    * the TTL and re-calculate the IP header checksum */
136   if (ifFrom == NULL)
137   {
138     PacketDecreaseTtlAndUpdateHeaderChecksum(buffer);
139   }
140
141   /* If the TTL is <= 0, do not forward this packet */
142   if (GetIpTtl(buffer) <= 0)
143   {
144     return;
145   }
146
147   /* Check if this packet was seen recently */
148   if (CheckMarkRecentPacket(buffer, len))
149   {
150     olsr_printf(
151       9,
152       "--> Discarding pkt: was a duplicate\n");
153     return;
154   }
155
156   /* Encapsulate and forward packet on all OLSR interfaces */
157   memset(&sin, 0, sizeof(sin));
158   sin.sin_family = AF_INET;
159   sin.sin_port = htons(BMF_ENCAP_PORT);
160
161   nextFwIntf = BmfInterfaces;
162   while (nextFwIntf != NULL)
163   {
164     struct TBmfInterface* fwIntf = nextFwIntf;
165     nextFwIntf = fwIntf->next;
166
167     if (fwIntf->olsrIntf != NULL)
168     {
169       int nBytesWritten;
170
171       /* Change source MAC address to that of myself */
172       memcpy(buffer + IFHWADDRLEN, fwIntf->macAddr, IFHWADDRLEN);
173
174       /* Destination address is local broadcast */
175       sin.sin_addr.s_addr = ((struct sockaddr_in*)&fwIntf->olsrIntf->int_broadaddr)->sin_addr.s_addr;
176
177       nBytesWritten = sendto(
178         fwIntf->encapsulatingSkfd,
179         buffer,
180         len,
181         MSG_DONTROUTE,
182         (struct sockaddr*) &sin,
183         sizeof(sin));                   
184
185       if (nBytesWritten != len)
186       {
187         olsr_printf(
188           1,
189           "%s: sendto() error forwarding MC pkt for %s to \"%s\": %s\n",
190           PLUGIN_NAME,
191           olsr_ip_to_string(&destIp),
192           fwIntf->olsrIntf->int_name,
193           strerror(errno));
194       }
195       else
196       {
197         olsr_printf(
198           9,
199           "Successfully encapsulated one MC pkt for %s to \"%s\"\n",
200           olsr_ip_to_string(&destIp),
201           fwIntf->olsrIntf->int_name);
202       } /* if (nBytesWritten != len) */
203     } /* if (fwIntf->olsrIntf != NULL) */
204   } /* while (nextFwIntf != NULL) */
205 }
206
207 static void BmfEncapsulatedPacketReceived(
208   struct TBmfInterface* intf, 
209   struct in_addr srcIp,
210   unsigned char* buffer,
211   ssize_t len)
212 {
213   union olsr_ip_addr fromAddr;
214   struct interface* ifFrom;
215   union olsr_ip_addr* forwarder;
216   int nBytesToWrite;
217   unsigned char* bufferToWrite;
218   int nBytesWritten;
219   int iAmMpr;
220   struct sockaddr_in sin;
221   struct TBmfInterface* nextFwIntf;
222
223   COPY_IP(&fromAddr, &srcIp.s_addr);
224
225   /* Are we talking to ourselves? */
226   if (if_ifwithaddr(&fromAddr) != NULL)
227   {
228     return;
229   }
230
231   /* Lookup the OLSR interface on which this packet is received */
232   ifFrom = intf->olsrIntf;
233
234   /* Encapsulated packet received on non-OLSR interface? Then discard */
235   if (ifFrom == NULL)
236   {
237     return;
238   }
239
240   /* Apply drop list? No, not needed: encapsulated packets are routed,
241    * so filtering should be done by adding a rule to the iptables FORWARD
242    * chain, e.g.:
243    * iptables -A FORWARD -m mac --mac-source 00:0C:29:28:0E:CC -j DROP */
244
245   /* Lookup main address of forwarding node */
246   forwarder = mid_lookup_main_addr(&fromAddr);
247   if (forwarder == NULL)
248   {
249     forwarder = &fromAddr;
250     olsr_printf(
251       9,
252       "Encapsulated MC pkt received; forwarder to me by %s on \"%s\"\n",
253       olsr_ip_to_string(forwarder),
254       intf->ifName);
255   }
256   else
257   {
258     olsr_printf(
259       9,
260       "Encapsulated MC pkt received; forwarder to me by %s (thru %s) on \"%s\"\n",
261       olsr_ip_to_string(forwarder),
262       olsr_ip_to_string(&fromAddr),
263       intf->ifName);
264   }
265
266   /* Check if this packet was seen recently */
267   if (CheckMarkRecentPacket(buffer, len))
268   {
269     olsr_printf(
270       9,
271       "--> Discarding encapsulated pkt: was a duplicate\n");
272     return;
273   }
274
275   /* Unpack encapsulated packet and send a copy to myself via the EtherTunTap device */
276   nBytesToWrite = len;
277   bufferToWrite = buffer;
278   if (TunOrTap == TT_TUN)
279   {
280     nBytesToWrite -= IP_HDR_OFFSET;
281     bufferToWrite += IP_HDR_OFFSET;
282   }
283   nBytesWritten = write(EtherTunTapFd, bufferToWrite, nBytesToWrite);
284   if (nBytesWritten != nBytesToWrite)
285   {
286     olsr_printf(
287       1,
288       "%s: write() error sending encapsulated MC pkt to \"%s\": %s\n",
289       PLUGIN_NAME,
290       EtherTunTapIfName,
291       strerror(errno));
292   }
293   else
294   {
295     olsr_printf(
296       9,
297       "Successfully unpacked and sent encapsulated MC pkt to \"%s\"\n",
298       EtherTunTapIfName);
299   }
300
301   /* Check if I am MPR for the forwarder */
302   iAmMpr = (olsr_lookup_mprs_set(forwarder) != NULL);
303   if (! iAmMpr)
304   {
305     olsr_printf(
306       9,
307       "--> Not forwarding encapsulated pkt: I am not selected as MPR by that forwarder\n");
308   }
309
310   memset(&sin, 0, sizeof(sin));
311   sin.sin_family = AF_INET;
312   sin.sin_port = htons(BMF_ENCAP_PORT);
313
314   nextFwIntf = BmfInterfaces;
315   while (nextFwIntf != NULL)
316   {
317     struct TBmfInterface* fwIntf = nextFwIntf;
318     nextFwIntf = fwIntf->next;
319
320     /* On non-OLSR interfaces: unpack encapsulated packet, decrease TTL
321      * and send */
322     if (fwIntf->olsrIntf == NULL)
323     {
324       struct TSaveTtl sttl;
325
326       /* Change source MAC address to that of sending interface */
327       memcpy(buffer + IFHWADDRLEN, fwIntf->macAddr, IFHWADDRLEN);
328
329       /* Save IP header checksum and the TTL-value of the packet, then 
330        * decrease the TTL by 1 before writing */
331       SaveTtlAndChecksum(buffer, &sttl);
332       PacketDecreaseTtlAndUpdateHeaderChecksum(buffer);
333
334       /* If the TTL is <= 0, do not forward this packet */
335       if (GetIpTtl(buffer) <= 0)
336       {
337         return;
338       }
339
340       nBytesWritten = write(fwIntf->capturingSkfd, buffer, len);
341       if (nBytesWritten != len)
342       {
343         olsr_printf(
344           1,
345           "%s: write() error sending unpacked encapsulated MC pkt to \"%s\": %s\n",
346           PLUGIN_NAME,
347           fwIntf->ifName,
348           strerror(errno));
349       }
350       else
351       {
352         olsr_printf(
353           9,
354           "Successfully unpacked and sent one encapsulated MC pkt to \"%s\"\n",
355           fwIntf->ifName);
356       }
357
358       /* Restore the IP header checksum and the TTL-value of the packet */
359       RestoreTtlAndChecksum(buffer, &sttl);
360     } /* if (fwIntf->olsrIntf == NULL) */
361
362     /* On OLSR interfaces, forward the packet if this node is selected as MPR by the 
363      * forwarding node */
364     else if (iAmMpr)
365     {
366       /* Change source MAC address to that of sending interface */
367       memcpy(buffer + IFHWADDRLEN, fwIntf->macAddr, IFHWADDRLEN);
368
369       /* Destination address is local broadcast */
370       sin.sin_addr.s_addr = ((struct sockaddr_in*)&fwIntf->olsrIntf->int_broadaddr)->sin_addr.s_addr;
371
372       nBytesWritten = sendto(
373         fwIntf->encapsulatingSkfd,
374         buffer,
375         len,
376         MSG_DONTROUTE,
377         (struct sockaddr*) &sin,
378         sizeof(sin));                   
379
380       if (nBytesWritten != len)
381       {
382         olsr_printf(
383           1,
384           "%s: sendto() error forwarding encapsulated MC pkt to \"%s\": %s\n",
385           PLUGIN_NAME,
386           fwIntf->olsrIntf->int_name,
387           strerror(errno));
388       }
389       else
390       {
391         olsr_printf(
392           9,
393           "Successfully forwarded one encapsulated MC pkt to \"%s\"\n",
394           fwIntf->olsrIntf->int_name);
395       }
396     } /* else if (iAmMpr) */
397   } /* while (nextFwIntf != NULL) */
398 }
399
400 static void DoBmf(void* useless)
401 {
402 #define BUFFER_MAX 2048
403   struct TBmfInterface* intf;
404   int nFdBitsSet;
405
406   /* Compose set of socket file descriptors. 
407    * Keep the highest descriptor seen. */
408   int highestSkfd = -1;
409   fd_set input_set;
410   FD_ZERO(&input_set);
411
412   intf = BmfInterfaces;
413   while (intf != NULL)
414   {
415     FD_SET(intf->capturingSkfd, &input_set);
416     if (intf->capturingSkfd > highestSkfd)
417     {
418       highestSkfd = intf->capturingSkfd;
419     }
420
421     if (intf->encapsulatingSkfd >= 0)
422     {
423       FD_SET(intf->encapsulatingSkfd, &input_set);
424       if (intf->encapsulatingSkfd > highestSkfd)
425       {
426         highestSkfd = intf->encapsulatingSkfd;
427       }
428     }
429
430     intf = intf->next;    
431   }
432
433   assert(highestSkfd >= 0);
434
435   /* Wait (blocking) for packets received on any of the sockets */
436   nFdBitsSet = select(highestSkfd + 1, &input_set, NULL, NULL, NULL);
437   if (nFdBitsSet < 0)
438   {
439     if (errno != EINTR)
440     {
441       olsr_printf(1, "%s: select() error: %s\n", PLUGIN_NAME, strerror(errno));
442     }
443     return;
444   }
445     
446   if (nFdBitsSet == 0)
447   {
448     /* No packets waiting. This is unexpected; normally we would excpect select(...)
449      * to return only if at least one packet was received (so nFdBitsSet > 0), or
450      * if this thread received a signal (so nFdBitsSet < 0). */
451     return;
452   }
453
454   while (nFdBitsSet > 0)
455   {
456     intf = BmfInterfaces;
457     while (intf != NULL)
458     {
459       int skfd = intf->capturingSkfd;
460       if (FD_ISSET(skfd, &input_set))
461       {
462         unsigned char buffer[BUFFER_MAX];
463         struct sockaddr_ll pktAddr;
464         socklen_t addrLen;
465         int nBytes;
466
467         /* A packet was captured */
468
469         nFdBitsSet--;
470
471         memset(&pktAddr, 0, sizeof(struct sockaddr_ll));
472         addrLen = sizeof(pktAddr);
473
474         nBytes = recvfrom(skfd, buffer, BUFFER_MAX, 0, (struct sockaddr*)&pktAddr, &addrLen);
475         if (nBytes < 0)
476         {
477           olsr_printf(1, "%s: recvfrom() error: %s\n", PLUGIN_NAME, strerror(errno));
478         }
479
480         /* Don't let BMF crash by sending too short packets. IP packets are always
481          * at least 14 (Ethernet header) + 20 (IP header) = 34 bytes long. */
482         if (nBytes >= 34)
483         {
484           if (pktAddr.sll_pkttype == PACKET_OUTGOING)
485           {
486             union olsr_ip_addr destIp;
487             COPY_IP(&destIp, buffer + IP_HDR_OFFSET + IPV4_OFFSET_DSTIP);
488             if (IsMulticast(&destIp) || IsLocalBroadcast(&destIp, intf->olsrIntf))
489             {
490               if (! IsOlsrOrBmfPacket(buffer, nBytes))
491               {
492                 MarkRecentPacket(buffer, nBytes);
493               }
494             }
495           }
496           else if (pktAddr.sll_pkttype == PACKET_MULTICAST ||
497                    pktAddr.sll_pkttype == PACKET_BROADCAST)
498           {
499             BmfPacketCaptured(intf, buffer, nBytes);
500           }
501         } /* if (nBytes >= 34) */
502       } /* if (FD_ISSET...) */
503
504       skfd = intf->encapsulatingSkfd;
505       if (skfd >= 0 && (FD_ISSET(skfd, &input_set)))
506       {
507         unsigned char buffer[BUFFER_MAX];
508         struct sockaddr_in addr;
509         socklen_t addrLen = sizeof(addr);
510         int nBytes;
511
512         /* An encapsulated packet was received */
513
514         nFdBitsSet--;
515
516         memset(&addr, 0, sizeof(addr));
517
518         nBytes = recvfrom(skfd, buffer, BUFFER_MAX, 0, (struct sockaddr*)&addr, &addrLen);
519         if (nBytes < 0)
520         {
521           olsr_printf(1, "%s: recvfrom() error: %s\n", PLUGIN_NAME, strerror(errno));
522         }
523         if (nBytes > 0)
524         {
525           BmfEncapsulatedPacketReceived(intf, addr.sin_addr, buffer, nBytes);
526         }
527       } /* if (skfd >= 0 && (FD_ISSET...) */
528
529       intf = intf->next;    
530     } /* while (intf != NULL) */
531   } /* while (nFdBitsSet > 0) */
532 }
533
534 static void BmfSignalHandler(int signo)
535 {
536   /* Dummy handler function */
537   return;
538 }
539
540 /* Thread entry function. Another thread can gracefully stop this thread by writing a
541  * '0' into global variable 'BmfThreadRunning' followed by sending a SIGALRM signal. */
542 static void* BmfRun(void* useless)
543 {
544   sigset_t blockedSigs;
545   sigfillset(&blockedSigs);
546   sigdelset(&blockedSigs, SIGALRM);
547   if (pthread_sigmask(SIG_BLOCK, &blockedSigs, NULL) < 0)
548   {
549     olsr_printf(1, "%s: pthread_sigmask() error: %s\n", PLUGIN_NAME, strerror(errno));
550   }
551
552   /* Set up the signal handler for the process: use SIGALRM to terminate
553    * the BMF thread. Only if a signal handler is specified, does a blocking
554    * system call return with errno set to EINTR; if a signal hander is not
555    * specified, any system call in which the thread may be waiting will not
556    * return. Note that the BMF thread is usually blocked in the select()
557    * function (see DoBmf()). */
558   if (signal(SIGALRM, BmfSignalHandler) == SIG_ERR)
559   {
560     olsr_printf(1, "%s: signal() error: %s\n", PLUGIN_NAME, strerror(errno));
561   }
562
563   /* Call the thread function until flagged to exit */
564   while (BmfThreadRunning != 0)
565   {
566     DoBmf(useless);
567   }
568   
569   return NULL;
570 }
571
572 /* Initialize the BMF plugin */
573 int InitBmf()
574 {
575   /* Check validity */
576   if (olsr_cnf->ip_version != AF_INET)
577   {
578     fprintf(stderr, PLUGIN_NAME ": This plugin only supports IPv4!\n");
579     return 0;
580   }
581
582   /* Clear the packet history */
583   InitPacketHistory();
584
585   if (CreateBmfNetworkInterfaces() < 0)
586   {
587     fprintf(stderr, PLUGIN_NAME ": could not initialize network interfaces!\n");
588     return 0;
589   }
590   
591   /* Run the multicast packet processing thread */
592   BmfThreadRunning = 1;
593   pthread_create(&BmfThread, NULL, BmfRun, NULL);
594
595   /* Register the duplicate registration pruning process */
596   olsr_register_scheduler_event(&PrunePacketHistory, NULL, 3.0, 2.0, NULL);
597
598   return 1;
599 }
600
601 /* Close the BMF plugin */
602 void CloseBmf()
603 {
604   /* Signal BmfThread to exit */
605   BmfThreadRunning = 0;
606   if (pthread_kill(BmfThread, SIGALRM) < 0)
607   /* Strangely enough, all running threads receive the SIGALRM signal. But only the
608    * BMF thread is affected by this signal, having specified a handler for this
609    * signal in its thread entry function BmfRun(...). */
610   {
611     olsr_printf(1, "%s: pthread_kill() error: %s\n", PLUGIN_NAME, strerror(errno));
612   }
613
614   /* Wait for BmfThread to acknowledge */
615   if (pthread_join(BmfThread, NULL) < 0)
616   {
617     olsr_printf(1, "%s: pthread_join() error: %s\n", PLUGIN_NAME, strerror(errno));
618   }
619
620   /* Time to clean up */
621   CloseBmfNetworkInterfaces();
622 }
623
624 int RegisterBmfParameter(char* key, char* value)
625 {
626   if (strcmp(key, "Drop") == 0)
627   {
628     return DropMac(value);
629   }
630   else if (strcmp(key, "NonOlsrIf") == 0)
631   {
632     return AddNonOlsrBmfIf(value);
633   }
634
635   return 0;
636 }