f8bf11b9b52f62cd0433ed969779f7691db2f2bc
[olsrd.git] / lib / p2pd / src / p2pd.c
1 /*
2  * The olsr.org Optimized Link-State Routing daemon(olsrd)
3  * Copyright (c) 2004-2009, the olsr.org team - see HISTORY file
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  *   notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  *   notice, this list of conditions and the following disclaimer in
14  *   the documentation and/or other materials provided with the
15  *   distribution.
16  * * Neither the name of olsr.org, olsrd nor the names of its
17  *   contributors may be used to endorse or promote products derived
18  *   from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * Visit http://www.olsr.org for more information.
34  *
35  * If you find this software useful feel free to make a donation
36  * to the project. For more information see the website or contact
37  * the copyright holders.
38  *
39  */
40
41
42 #include "p2pd.h"
43
44 /* System includes */
45 #include <stddef.h>             /* NULL */
46 #include <sys/types.h>          /* ssize_t */
47 #include <string.h>             /* strerror() */
48 #include <stdarg.h>             /* va_list, va_start, va_end */
49 #include <errno.h>              /* errno */
50 #include <assert.h>             /* assert() */
51 #include <unistd.h>
52 #include <fcntl.h>
53 #include <linux/if_ether.h>     /* ETH_P_IP */
54 #include <linux/if_packet.h>    /* struct sockaddr_ll, PACKET_MULTICAST */
55 //#include <pthread.h> /* pthread_t, pthread_create() */
56 #include <signal.h>             /* sigset_t, sigfillset(), sigdelset(), SIGINT */
57 #include <netinet/ip.h>         /* struct ip */
58 #include <netinet/udp.h>        /* struct udphdr */
59 #include <unistd.h>             /* close() */
60
61 #include <netinet/in.h>
62 #include <netinet/ip6.h>
63
64 #include <time.h>
65
66 /* OLSRD includes */
67 #include "plugin_util.h"        /* set_plugin_int */
68 #include "defs.h"               /* olsr_cnf, //OLSR_PRINTF */
69 #include "ipcalc.h"
70 #include "olsr.h"               /* //OLSR_PRINTF */
71 #include "mid_set.h"            /* mid_lookup_main_addr() */
72 #include "link_set.h"           /* get_best_link_to_neighbor() */
73 #include "net_olsr.h"           /* ipequal */
74 #include "parser.h"
75
76 /* plugin includes */
77 #include "NetworkInterfaces.h"  /* NonOlsrInterface,
78                                    CreateBmfNetworkInterfaces(),
79                                    CloseBmfNetworkInterfaces() */
80 //#include "Address.h"          /* IsMulticast() */
81 #include "Packet.h"             /* ENCAP_HDR_LEN,
82                                    BMF_ENCAP_TYPE,
83                                    BMF_ENCAP_LEN etc. */
84 #include "PacketHistory.h"
85 #include "dllist.h"
86
87 int P2pdTtl                        = 0;
88 int P2pdUseHash                    = 0;  /* Switch off hash filter by default */
89 int P2pdDuplicateTimeout           = P2PD_VALID_TIME;
90
91 /* List of UDP destination address and port information */
92 struct UdpDestPort *                 UdpDestPortList = NULL;
93
94 /* List of filter entries to check for duplicate messages
95  */
96 struct node *                        dupFilterHead = NULL;
97 struct node *                        dupFilterTail = NULL;
98
99 bool is_broadcast(const struct sockaddr_in addr);
100 bool is_multicast(const struct sockaddr_in addr);
101 char * get_ipv4_str(uint32_t address, char *s, size_t maxlen);
102 char * get_ipv6_str(unsigned char* address, char *s, size_t maxlen);
103 void dump_packet(unsigned char* packet, int length);
104 bool check_and_mark_recent_packet(unsigned char *data, int len);
105
106 /* -------------------------------------------------------------------------
107  * Function   : PacketReceivedFromOLSR
108  * Description: Handle a received packet from a OLSR message
109  * Input      : ipPacket into an unsigned char and the lenght of the packet
110  * Output     : none
111  * Return     : none
112  * Data Used  : BmfInterfaces
113  * ------------------------------------------------------------------------- */
114 static void
115 PacketReceivedFromOLSR(unsigned char *encapsulationUdpData, int len)
116 {
117   struct ip *ipHeader;        /* IP header inside the encapsulated IP packet */
118   struct ip6_hdr *ip6Header;  /* IP header inside the encapsulated IP packet */
119   struct udphdr *udpHeader;
120   struct NonOlsrInterface *walker;
121   int stripped_len = 0;
122   union olsr_ip_addr destAddr;
123   int destPort;
124   bool isInList = false;
125
126   ipHeader = (struct ip *) ARM_NOWARN_ALIGN(encapsulationUdpData);
127   ip6Header = (struct ip6_hdr *) ARM_NOWARN_ALIGN(encapsulationUdpData);
128   //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN got packet from OLSR message\n");
129
130   if (check_and_mark_recent_packet(encapsulationUdpData, len))
131     return;
132
133   /* Check with each network interface what needs to be done on it */
134   for (walker = nonOlsrInterfaces; walker != NULL; walker = walker->next) {
135     /* To a non-OLSR interface: unpack encapsulated IP packet and forward it */
136     if (walker->olsrIntf == NULL) {
137       int nBytesWritten;
138       struct sockaddr_ll dest;
139
140       memset(&dest, 0, sizeof(dest));
141       dest.sll_family = AF_PACKET;
142       if ((encapsulationUdpData[0] & 0xf0) == 0x40) {
143         dest.sll_protocol = htons(ETH_P_IP);
144         stripped_len = ntohs(ipHeader->ip_len);
145       }
146
147       if ((encapsulationUdpData[0] & 0xf0) == 0x60) {
148         dest.sll_protocol = htons(ETH_P_IPV6);
149         stripped_len = 40 + ntohs(ip6Header->ip6_plen); // IPv6 Header size (40)
150                                                         // + payload_len
151       }
152
153       // Sven-Ola: Don't know how to handle the "stripped_len is uninitialized"
154       // condition, maybe exit(1) is better...?
155       if (0 == stripped_len)
156         return;
157
158       //TODO: if packet is not IP die here
159
160       if (stripped_len > len) {
161       }
162
163       dest.sll_ifindex = if_nametoindex(walker->ifName);
164       dest.sll_halen = IFHWADDRLEN;
165
166       if (olsr_cnf->ip_version == AF_INET) {
167         /* Use all-ones as destination MAC address. When the IP destination is
168          * a multicast address, the destination MAC address should normally also
169          * be a multicast address. E.g., when the destination IP is 224.0.0.1,
170          * the destination MAC should be 01:00:5e:00:00:01. However, it does not
171          * seem to matter when the destination MAC address is set to all-ones
172          * in that case.
173          */
174
175         if (IsMulticastv4(ipHeader)) {
176           dest.sll_addr[0] = 0x01;
177           dest.sll_addr[1] = 0x00;
178           dest.sll_addr[2] = 0x5E;
179           dest.sll_addr[3] = (ipHeader->ip_dst.s_addr >> 16) & 0x7F;
180           dest.sll_addr[4] = (ipHeader->ip_dst.s_addr >> 8) & 0xFF;
181           dest.sll_addr[5] = ipHeader->ip_dst.s_addr & 0xFF;
182         } else /* if (IsBroadcast(ipHeader)) */ {
183           memset(dest.sll_addr, 0xFF, IFHWADDRLEN);
184         }
185       } else /*(olsr_cnf->ip_version == AF_INET6) */ {
186         if (IsMulticastv6(ip6Header)) {
187           dest.sll_addr[0] = 0x33;
188           dest.sll_addr[1] = 0x33;
189           dest.sll_addr[2] = ip6Header->ip6_dst.s6_addr[12];
190           dest.sll_addr[3] = ip6Header->ip6_dst.s6_addr[13];
191           dest.sll_addr[4] = ip6Header->ip6_dst.s6_addr[14];
192           dest.sll_addr[5] = ip6Header->ip6_dst.s6_addr[15];
193         }
194       }
195
196       if (olsr_cnf->ip_version == AF_INET) {
197         // Determine the IP address and the port from the header information
198         if (ipHeader->ip_p == SOL_UDP && !IsIpv4Fragment(ipHeader)) {
199           udpHeader = (struct udphdr*) ARM_NOWARN_ALIGN((encapsulationUdpData +
200                                        GetIpHeaderLength(encapsulationUdpData)));
201           destAddr.v4.s_addr = ipHeader->ip_dst.s_addr;
202           destPort = htons(udpHeader->dest);
203           isInList = InUdpDestPortList(AF_INET, &destAddr, destPort);
204 #ifdef INCLUDE_DEBUG_OUTPUT
205           if (!isInList) {
206             char tmp[32];
207             OLSR_PRINTF(1,
208                         "%s: Not in dest/port list: %s:%d\n",
209                         PLUGIN_NAME_SHORT,
210                         get_ipv4_str(destAddr.v4.s_addr,
211                                      tmp,
212                                      sizeof(tmp)),
213                         destPort);
214           }
215 #endif
216         }
217       } else /* (olsr_cnf->ip_version == AF_INET6) */ {
218         if (ip6Header->ip6_nxt == SOL_UDP && !IsIpv6Fragment(ip6Header)) {
219           udpHeader = (struct udphdr*) ARM_NOWARN_ALIGN((encapsulationUdpData + 40));
220           memcpy(&destAddr.v6, &ip6Header->ip6_dst, sizeof(struct in6_addr));
221           destPort = htons(udpHeader->dest);
222           isInList = InUdpDestPortList(AF_INET6, &destAddr, destPort);
223 #ifdef INCLUDE_DEBUG_OUTPUT
224           if (!isInList) {
225             char tmp[64];
226             OLSR_PRINTF(1,
227                         "%s: Not in dest/port list: %s:%d\n",
228                         PLUGIN_NAME_SHORT,
229                         get_ipv6_str(destAddr.v6.s6_addr,
230                                      tmp,
231                                      sizeof(tmp)),
232                         destPort);
233           }
234 #endif
235         }
236       }
237
238       if (!isInList) {
239         /* Address/port combination of this packet is not in the UDP dest/port
240          * list and will therefore be suppressed. I.e. just continue with the
241          * next interface to emit on.
242          */
243         continue;
244       }
245       
246       nBytesWritten = sendto(walker->capturingSkfd,
247                              encapsulationUdpData,
248                              stripped_len,
249                              0,
250                              (struct sockaddr *)&dest,
251                              sizeof(dest));
252       if (nBytesWritten != stripped_len) {
253         P2pdPError("sendto() error forwarding unpacked encapsulated pkt on \"%s\"",
254                    walker->ifName);
255       } else {
256
257         //OLSR_PRINTF(
258         //  2,
259         //  "%s: --> unpacked and forwarded on \"%s\"\n",
260         //  PLUGIN_NAME_SHORT,
261         //  walker->ifName);
262       }
263     }                           /* if (walker->olsrIntf == NULL) */
264   }
265 }                               /* PacketReceivedFromOLSR */
266
267 /* Highest-numbered open socket file descriptor. To be used as first
268  * parameter in calls to select(...).
269  */
270 int HighestSkfd = -1;
271
272 /* Set of socket file descriptors */
273 fd_set InputSet;
274
275 /* -------------------------------------------------------------------------
276  * Function   : p2pd_message_seen
277  * Description: Check whether the current message has been seen before
278  * Input      : head - start of the list to check for the message
279  *              tail - end of the list to check for the message
280  *              m    - message to check for in the list
281  * Output     : none
282  * Return     : true if message was found, false otherwise
283  * Data Used  : P2pdDuplicateTimeout
284  * ------------------------------------------------------------------------- */
285 bool
286 p2pd_message_seen(struct node **head, struct node **tail, union olsr_message *m)
287 {
288   struct node * curr;
289   time_t now;
290
291   now = time(NULL);
292
293   // Check whether any entries have aged
294   curr = *head;
295   while (curr) {
296     struct DupFilterEntry *filter;
297     struct node * next = curr->next; // Save the current pointer since curr may
298                                      // be destroyed
299
300     filter = (struct DupFilterEntry*)curr->data;
301
302     if ((filter->creationtime + P2pdDuplicateTimeout) < now)
303       remove_node(head, tail, curr, true);
304
305     // Skip to the next element
306     curr = next;
307   }
308
309   // Now check whether there are any duplicates
310   for (curr = *head; curr; curr = curr->next) {
311     struct DupFilterEntry *filter = (struct DupFilterEntry*)curr->data;
312
313     if (olsr_cnf->ip_version == AF_INET) {
314       if (filter->address.v4.s_addr == m->v4.originator &&
315           filter->msgtype == m->v4.olsr_msgtype &&
316           filter->seqno == m->v4.seqno) {
317           return true;
318       }
319     } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
320       if (memcmp(filter->address.v6.s6_addr,
321                  m->v6.originator.s6_addr,
322                  sizeof(m->v6.originator.s6_addr)) == 0 &&
323           filter->msgtype == m->v6.olsr_msgtype &&
324           filter->seqno == m->v6.seqno) {
325           return true;
326       }
327     }
328   }
329
330   return false;
331 }
332
333 /* -------------------------------------------------------------------------
334  * Function   : p2pd_store_message
335  * Description: Store a new message in the duplicate message check list
336  * Input      : head - start of the list to add the message to
337  *              tail - end of the list to add the message to
338  *              m    - message to add to the list
339  * Output     : none
340  * Return     : nothing
341  * Data Used  : none
342  * ------------------------------------------------------------------------- */
343 void
344 p2pd_store_message(struct node **head, struct node **tail, union olsr_message *m)
345 {
346   time_t now;
347
348   // Store a message into the database
349   struct DupFilterEntry *new_dup = calloc(1, sizeof(struct DupFilterEntry));
350   if (new_dup == NULL) {
351     OLSR_PRINTF(1, "%s: Out of memory\n", PLUGIN_NAME_SHORT);
352     return;
353   }
354
355   now = time(NULL);
356
357   new_dup->creationtime = now;
358   if (olsr_cnf->ip_version == AF_INET) {
359     new_dup->address.v4.s_addr = m->v4.originator;
360     new_dup->msgtype           = m->v4.olsr_msgtype;
361     new_dup->seqno             = m->v4.seqno;
362   } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
363     memcpy(new_dup->address.v6.s6_addr,
364            m->v6.originator.s6_addr,
365            sizeof(m->v6.originator.s6_addr));
366     new_dup->msgtype           = m->v6.olsr_msgtype;
367     new_dup->seqno             = m->v6.seqno;
368   }
369
370   // Add the element to the head of the list
371   append_node(head, tail, new_dup);
372 }
373
374 /* -------------------------------------------------------------------------
375  * Function   : p2pd_is_duplicate_message
376  * Description: Check whether the specified message is a duplicate
377  * Input      : msg - message to check for in the list of duplicate messages
378  * Output     : none
379  * Return     : true if message was found, false otherwise
380  * Data Used  : none
381  * ------------------------------------------------------------------------- */
382 bool
383 p2pd_is_duplicate_message(union olsr_message *msg)
384 {
385   if(p2pd_message_seen(&dupFilterHead, &dupFilterTail, msg)) {
386     return true;
387   }
388
389   p2pd_store_message(&dupFilterHead, &dupFilterTail, msg);
390
391   return false;
392 }
393
394 /* -------------------------------------------------------------------------
395  * Function   : olsr_parser
396  * Description: Function to be passed to the parser engine. This function
397  *              processes the incoming message and passes it on if necessary.
398  * Input      : m      - message to parse
399  *              in_if  - interface to use (unused in this application)
400  *              ipaddr - IP-address to use (unused in this application)
401  * Output     : none
402  * Return     : false if message should be supressed, true otherwise
403  * Data Used  : none
404  * ------------------------------------------------------------------------- */
405 bool
406 olsr_parser(union olsr_message *m,
407             struct interface *in_if __attribute__ ((unused)),
408             union olsr_ip_addr *ipaddr __attribute__ ((unused)))
409 {
410   union olsr_ip_addr originator;
411   int size;
412
413   //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN: Received msg in parser\n");
414
415         /* Fetch the originator of the messsage */
416   if (olsr_cnf->ip_version == AF_INET) {
417     memcpy(&originator, &m->v4.originator, olsr_cnf->ipsize);
418     size = ntohs(m->v4.olsr_msgsize);
419   } else {
420     memcpy(&originator, &m->v6.originator, olsr_cnf->ipsize);
421     size = ntohs(m->v6.olsr_msgsize);
422   }
423
424   /* Check if message originated from this node.
425    *         If so - back off */
426   if (ipequal(&originator, &olsr_cnf->main_addr))
427     return false;          /* Don't forward either */
428
429   /* Check for duplicate messages for processing */
430   if (p2pd_is_duplicate_message(m))
431     return true;  /* Don't process but allow to be forwarded */
432
433   if (olsr_cnf->ip_version == AF_INET) {
434     PacketReceivedFromOLSR((unsigned char *)&m->v4.message, size - 12);
435   } else {
436     PacketReceivedFromOLSR((unsigned char *)&m->v6.message, size - 12 - 96);
437   }
438
439         return true;
440 }
441
442 /* -------------------------------------------------------------------------
443  * Function   : olsr_p2pd_gen
444  * Description: Sends a packet in the OLSR network
445  * Input      : packet - packet to send in the OLSR network
446  *              len    - length of the packet to send
447  * Output     : none
448  * Return     : nothing
449  * Data Used  : none
450  * ------------------------------------------------------------------------- */
451 void
452 olsr_p2pd_gen(unsigned char *packet, int len)
453 {
454   /* send buffer: huge */
455   char buffer[10240];
456   int aligned_size;
457   union olsr_message *message = (union olsr_message *)buffer;
458   struct interface *ifn;
459
460   aligned_size=len;
461
462   if ((aligned_size % 4) != 0) {
463     aligned_size = (aligned_size - (aligned_size % 4)) + 4;
464   }
465
466   /* fill message */
467   if (olsr_cnf->ip_version == AF_INET) {
468     /* IPv4 */
469     message->v4.olsr_msgtype  = P2PD_MESSAGE_TYPE;
470     message->v4.olsr_vtime    = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
471     memcpy(&message->v4.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
472     message->v4.ttl           = P2pdTtl ? P2pdTtl : MAX_TTL;
473     message->v4.hopcnt        = 0;
474     message->v4.seqno         = htons(get_msg_seqno());
475     message->v4.olsr_msgsize  = htons(aligned_size + 12);
476     memset(&message->v4.message, 0, aligned_size);
477     memcpy(&message->v4.message, packet, len);
478     aligned_size = aligned_size + 12;
479   } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
480     /* IPv6 */
481     message->v6.olsr_msgtype  = P2PD_MESSAGE_TYPE;
482     message->v6.olsr_vtime    = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
483     memcpy(&message->v6.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
484     message->v6.ttl           = P2pdTtl ? P2pdTtl : MAX_TTL;
485     message->v6.hopcnt        = 0;
486     message->v6.seqno         = htons(get_msg_seqno());
487     message->v6.olsr_msgsize  = htons(aligned_size + 12 + 96);
488     memset(&message->v6.message, 0, aligned_size);
489     memcpy(&message->v6.message, packet, len);
490     aligned_size = aligned_size + 12 + 96;
491   }
492
493   /* looping through interfaces */
494   for (ifn = ifnet; ifn; ifn = ifn->int_next) {
495     //OLSR_PRINTF(1, "%s: Generating packet - [%s]\n", PLUGIN_NAME_SHORT, ifn->int_name);
496
497     if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
498       /* send data and try again */
499       net_output(ifn);
500       if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
501         //OLSR_PRINTF(1, "%s: could not send on interface: %s\n", PLUGIN_NAME_SHORT, ifn->int_name);
502       }
503     }
504   }
505 }
506
507 /* -------------------------------------------------------------------------
508  * Function   : P2pdPError
509  * Description: Prints an error message at OLSR debug level 1.
510  *              First the plug-in name is printed. Then (if format is not NULL
511  *              and *format is not empty) the arguments are printed, followed
512  *              by a colon and a blank. Then the message and a new-line.
513  * Input      : format, arguments
514  * Output     : none
515  * Return     : none
516  * Data Used  : none
517  * ------------------------------------------------------------------------- */
518 void
519 P2pdPError(const char *format, ...)
520 {
521 #define MAX_STR_DESC 255
522   char strDesc[MAX_STR_DESC];
523
524 #if !defined REMOVE_LOG_DEBUG
525   char *stringErr = strerror(errno);
526 #endif
527
528   /* Rely on short-circuit boolean evaluation */
529   if (format == NULL || *format == '\0') {
530     //OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", PLUGIN_NAME, stringErr);
531   } else {
532     va_list arglist;
533
534     va_start(arglist, format);
535     vsnprintf(strDesc, MAX_STR_DESC, format, arglist);
536     va_end(arglist);
537
538     strDesc[MAX_STR_DESC - 1] = '\0';   /* Ensures null termination */
539
540 #if !defined REMOVE_LOG_DEBUG
541     OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", strDesc, stringErr);
542 #endif
543   }
544 }                               /* P2pdPError */
545
546 /* -------------------------------------------------------------------------
547  * Function   : MainAddressOf
548  * Description: Lookup the main address of a node
549  * Input      : ip - IP address of the node
550  * Output     : none
551  * Return     : The main IP address of the node
552  * Data Used  : none
553  * ------------------------------------------------------------------------- */
554 union olsr_ip_addr *
555 MainAddressOf(union olsr_ip_addr *ip)
556 {
557   union olsr_ip_addr *result;
558
559   /* TODO: mid_lookup_main_addr() is not thread-safe! */
560   result = mid_lookup_main_addr(ip);
561   if (result == NULL) {
562     result = ip;
563   }
564   return result;
565 }                               /* MainAddressOf */
566
567
568 /* -------------------------------------------------------------------------
569  * Function   : InUdpDestPortList
570  * Description: Check whether the specified address and port is in the list of
571  *              configured UDP destination/port entries
572  * Input      : ip_version  - IP version to use for this check
573  *              addr        - address to check for in the list
574  *              port        - port to check for in the list
575  * Output     : none
576  * Return     : true if destination/port combination was found, false otherwise
577  * Data Used  : UdpDestPortList
578  * ------------------------------------------------------------------------- */
579 bool
580 InUdpDestPortList(int ip_version, union olsr_ip_addr *addr, uint16_t port)
581 {
582   struct UdpDestPort *walker;
583
584   for (walker = UdpDestPortList; walker; walker = walker->next) {
585     if (walker->ip_version == ip_version) {
586       if (ip_version == AF_INET) {
587         if (addr->v4.s_addr == walker->address.v4.s_addr &&
588             walker->port == port)
589           return true;  // Found so we can stop here
590       } else /* ip_version == AF_INET6 */ {
591         if ((memcmp(addr->v6.s6_addr,
592                    walker->address.v6.s6_addr,
593                    sizeof(addr->v6.s6_addr)) == 0) &&
594             (walker->port == port))
595           return true;  // Found so we can stop here
596       }
597     }
598   }
599   return false;
600 }
601
602 /* -------------------------------------------------------------------------
603  * Function   : P2pdPacketCaptured
604  * Description: Handle a captured IP packet
605  * Input      : encapsulationUdpData - space for the encapsulation header,
606  *              followed by the captured IP packet
607  *              nBytes - The number of bytes in the data packet
608  * Output     : none
609  * Return     : none
610  * Data Used  : P2pdInterfaces
611  * Notes      : The IP packet is assumed to be captured on a socket of family
612  *              PF_PACKET and type SOCK_DGRAM (cooked).
613  * ------------------------------------------------------------------------- */
614 static void
615 P2pdPacketCaptured(unsigned char *encapsulationUdpData, int nBytes)
616 {
617   union olsr_ip_addr dst;      /* Destination IP address in captured packet */
618   struct ip *ipHeader;         /* The IP header inside the captured IP packet */
619   struct ip6_hdr *ipHeader6;   /* The IP header inside the captured IP packet */
620   struct udphdr *udpHeader;
621   u_int16_t destPort;
622
623   if ((encapsulationUdpData[0] & 0xf0) == 0x40) {       //IPV4
624
625     ipHeader = (struct ip *) ARM_NOWARN_ALIGN(encapsulationUdpData);
626
627     dst.v4 = ipHeader->ip_dst;
628
629     if (ipHeader->ip_p != SOL_UDP) {
630       /* Not UDP */
631 #ifdef INCLUDE_DEBUG_OUTPUT
632       OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
633 #endif
634       return;                   /* for */
635     }
636
637     // If we're dealing with a fragment we bail out here since there's no valid
638     // UDP header in this message
639     if (IsIpv4Fragment(ipHeader)) {
640 #ifdef INCLUDE_DEBUG_OUTPUT
641       OLSR_PRINTF(1, "%s: Is IPv4 fragment\n", PLUGIN_NAME_SHORT);
642 #endif
643       return;
644     }
645
646     if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
647       return;
648
649     udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData +
650                                   GetIpHeaderLength(encapsulationUdpData)));
651     destPort = ntohs(udpHeader->dest);
652
653     if (!InUdpDestPortList(AF_INET, &dst, destPort)) {
654 #ifdef INCLUDE_DEBUG_OUTPUT
655       char tmp[32];
656       OLSR_PRINTF(1, "%s: Not in dest/port list: %s:%d\n", PLUGIN_NAME_SHORT,
657                   get_ipv4_str(dst.v4.s_addr, tmp, sizeof(tmp)), destPort);
658 #endif
659        return;
660     }
661   }                            //END IPV4
662   else if ((encapsulationUdpData[0] & 0xf0) == 0x60) {  //IPv6
663
664     ipHeader6 = (struct ip6_hdr *) ARM_NOWARN_ALIGN(encapsulationUdpData);
665
666     memcpy(&dst.v6, &ipHeader6->ip6_dst, sizeof(struct in6_addr));
667
668     if (ipHeader6->ip6_dst.s6_addr[0] == 0xff)  //Multicast
669     {
670       //Continue
671     } else {
672       return;                   //not multicast
673     }
674     if (ipHeader6->ip6_nxt != SOL_UDP) {
675       /* Not UDP */
676       //OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
677       return;                   /* for */
678     }
679
680     // Check whether this is a IPv6 fragment
681     if (IsIpv6Fragment(ipHeader6)) {
682 #ifdef INCLUDE_DEBUG_OUTPUT
683       OLSR_PRINTF(1, "%s: Is IPv6 fragment\n", PLUGIN_NAME_SHORT);
684 #endif
685       return;
686     }
687
688     if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
689       return;
690
691     udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData + 40));
692     destPort = ntohs(udpHeader->dest);
693
694     if (!InUdpDestPortList(AF_INET6, &dst, destPort)) {
695 #ifdef INCLUDE_DEBUG_OUTPUT
696       char tmp[64];
697       OLSR_PRINTF(1, "%s: Not in dest/port list: %s:%d\n", PLUGIN_NAME_SHORT,
698                   get_ipv6_str(dst.v6.s6_addr, tmp, sizeof(tmp)), destPort);
699 #endif
700       return;
701     }
702   }                             //END IPV6
703   else {
704     return;                     //Is not IP packet
705   }
706
707   // send the packet to OLSR forward mechanism
708   olsr_p2pd_gen(encapsulationUdpData, nBytes);
709 }                               /* P2pdPacketCaptured */
710
711
712 /* -------------------------------------------------------------------------
713  * Function   : DoP2pd
714  * Description: This function is registered with the OLSR scheduler and called
715  *              when something is captured
716  * Input      : none
717  * Output     : none
718  * Return     : none
719  * Data Used  :
720  * ------------------------------------------------------------------------- */
721 void
722 DoP2pd(int skfd,
723        void *data __attribute__ ((unused)),
724        unsigned int flags __attribute__ ((unused)))
725 {
726   unsigned char rxBuffer[P2PD_BUFFER_SIZE];
727   if (skfd >= 0) {
728     struct sockaddr_ll pktAddr;
729     socklen_t addrLen = sizeof(pktAddr);
730     int nBytes;
731     unsigned char *ipPacket;
732
733     /* Receive the captured Ethernet frame, leaving space for the BMF
734      * encapsulation header */
735     ipPacket = GetIpPacket(rxBuffer);
736     nBytes = recvfrom(skfd, ipPacket, P2PD_BUFFER_SIZE,
737                       0, (struct sockaddr *)&pktAddr, &addrLen);
738 #ifdef INCLUDE_DEBUG_OUTPUT
739     OLSR_PRINTF(1, "%s: Received %d bytes\n", PLUGIN_NAME_SHORT, nBytes);
740 #endif
741
742     if (nBytes < 0) {
743
744       return;                   /* for */
745     }
746
747     /* if (nBytes < 0) */
748     /* Check if the number of received bytes is large enough for an IP
749      * packet which contains at least a minimum-size IP header.
750      * Note: There is an apparent bug in the packet socket implementation in
751      * combination with VLAN interfaces. On a VLAN interface, the value returned
752      * by 'recvfrom' may (but need not) be 4 (bytes) larger than the value
753      * returned on a non-VLAN interface, for the same ethernet frame. */
754     if (nBytes < (int)sizeof(struct ip)) {
755       ////OLSR_PRINTF(
756       //              1,
757       //              "%s: captured frame too short (%d bytes) on \"%s\"\n",
758       //              PLUGIN_NAME_SHORT,
759       //              nBytes,
760       //              walker->ifName);
761
762       return;                   /* for */
763     }
764
765     if (pktAddr.sll_pkttype == PACKET_OUTGOING ||
766         pktAddr.sll_pkttype == PACKET_MULTICAST ||
767         pktAddr.sll_pkttype == PACKET_BROADCAST) {
768 #ifdef INCLUDE_DEBUG_OUTPUT
769       OLSR_PRINTF(1, "%s: Multicast or broadcast packet was captured.\n",
770                   PLUGIN_NAME_SHORT);
771       dump_packet(ipPacket, nBytes);
772 #endif
773       /* A multicast or broadcast packet was captured */
774       P2pdPacketCaptured(ipPacket, nBytes);
775
776     }                           /* if (pktAddr.sll_pkttype == ...) */
777   }                             /* if (skfd >= 0 && (FD_ISSET...)) */
778 }                               /* DoP2pd */
779
780 /* -------------------------------------------------------------------------
781  * Function   : InitP2pd
782  * Description: Initialize the P2pd plugin
783  * Input      : skipThisInterface - pointer to interface to skip
784  * Output     : none
785  * Return     : Always 0
786  * Data Used  : none
787  * ------------------------------------------------------------------------- */
788 int
789 InitP2pd(struct interface *skipThisIntf)
790 {
791   if (P2pdUseHash) {
792     // Initialize hash table for hash based duplicate IP packet check
793     InitPacketHistory();
794   }
795
796   //Tells OLSR to launch olsr_parser when the packets for this plugin arrive
797   //olsr_parser_add_function(&olsr_parser, PARSER_TYPE,1);
798   olsr_parser_add_function(&olsr_parser, PARSER_TYPE);
799
800   //Creates captures sockets and register them to the OLSR scheduler
801   CreateNonOlsrNetworkInterfaces(skipThisIntf);
802
803   return 0;
804 }                               /* InitP2pd */
805
806 /* -------------------------------------------------------------------------
807  * Function   : CloseP2pd
808  * Description: Close the P2pd plugin and clean up
809  * Input      : none
810  * Output     : none
811  * Return     : none
812  * Data Used  :
813  * ------------------------------------------------------------------------- */
814 void
815 CloseP2pd(void)
816 {
817   CloseNonOlsrNetworkInterfaces();
818 }
819
820 /* -------------------------------------------------------------------------
821  * Function   : SetP2pdTtl
822  * Description: Set the TTL for message from this plugin
823  * Input      : value - parameter value to evaluate
824  * Output     : none
825  * Return     : Always 0
826  * Data Used  : P2pdTtl
827  * ------------------------------------------------------------------------- */
828 int
829 SetP2pdTtl(const char *value,
830            void *data __attribute__ ((unused)),
831            set_plugin_parameter_addon addon __attribute__ ((unused)))
832 {
833   assert(value != NULL);
834   P2pdTtl = atoi(value);
835
836   return 0;
837 }
838
839 /* -------------------------------------------------------------------------
840  * Function   : SetP2pdUseHashFilter
841  * Description: Set the Hash filter flag for this plug-in
842  * Input      : value - parameter value to evaluate
843  *              data  - data associated with this parameter (unused in this app)
844  *              addon - additional parameter data
845  * Output     : none
846  * Return     : Always 0
847  * Data Used  : P2pdUseHash
848  * ------------------------------------------------------------------------- */
849 int
850 SetP2pdUseHashFilter(const char *value,
851                      void *data __attribute__ ((unused)),
852                      set_plugin_parameter_addon addon __attribute__ ((unused)))
853 {
854   assert(value != NULL);
855   P2pdUseHash = atoi(value);
856   
857   return 0;
858 }
859
860 /* -------------------------------------------------------------------------
861  * Function   : AddUdpDestPort
862  * Description: Set the UDP destination/port combination as an entry in the
863  *              UdpDestPortList
864  * Input      : value - parameter value to evaluate
865  * Output     : none
866  * Return     : -1 on error condition, 0 if all is ok
867  * Data Used  : UdpDestPortList
868  * ------------------------------------------------------------------------- */
869 int
870 AddUdpDestPort(const char *value,
871                void *data __attribute__ ((unused)),
872                set_plugin_parameter_addon addon __attribute__ ((unused)))
873 {
874   char destAddr[INET6_ADDRSTRLEN];
875   uint16_t destPort;
876   int num;
877   struct UdpDestPort *    new;
878   struct sockaddr_in      addr4;
879   struct sockaddr_in6     addr6;
880   int                     ip_version    = AF_INET;
881   int                     res;
882
883   assert(value != NULL);
884
885   // Retrieve the data from the argument string passed
886   memset(destAddr, 0, sizeof(destAddr));
887   num = sscanf(value, "%45s %hd", destAddr, &destPort);
888   if (num != 2) {
889     OLSR_PRINTF(1, "%s: Invalid argument for \"UdpDestPort\"",
890                 PLUGIN_NAME_SHORT);
891     return -1;
892   }
893
894   // Check whether we're dealing with an IPv4 or IPv6 address
895   // When the string contains a ':' we can assume we're dealing with IPv6
896   if (strchr(destAddr, (int)':')) {
897     ip_version = AF_INET6;
898   }
899
900   // Check whether the specified address was either IPv4 multicast,
901   // IPv4 broadcast or IPv6 multicast.
902
903   switch (ip_version) {
904   case AF_INET:
905     res = inet_pton(AF_INET, destAddr, &addr4.sin_addr);
906     if (!is_broadcast(addr4) && !is_multicast(addr4)) {
907       OLSR_PRINTF(1,"WARNING: IPv4 address must be multicast or broadcast... ");
908     }
909     break;
910   case AF_INET6:
911     res = inet_pton(AF_INET6, destAddr, &addr6.sin6_addr);
912     if (addr6.sin6_addr.s6_addr[0] != 0xFF) {
913       OLSR_PRINTF(1,"WARNING: IPv6 address must be multicast... ");
914       return -1;
915     }
916     break;
917   }
918   // Determine whether it is a valid IP address
919   if (res == 0) {
920     OLSR_PRINTF(1, "Invalid address specified for \"UdpDestPort\"");
921     return -1;
922   }
923
924   // Create a new entry and link it into the chain
925   new = calloc(1, sizeof(struct UdpDestPort));
926   if (new == NULL) {
927     OLSR_PRINTF(1, "%s: Out of memory", PLUGIN_NAME_SHORT);
928     return -1;
929   }
930
931   new->ip_version = ip_version;
932   switch (ip_version) {
933   case AF_INET:
934     new->address.v4.s_addr = addr4.sin_addr.s_addr;
935     break;
936   case AF_INET6:
937     memcpy(&new->address.v6.s6_addr,
938            &addr6.sin6_addr.s6_addr,
939            sizeof(addr6.sin6_addr.s6_addr));
940     break;
941   }
942   new->port = destPort;
943   new->next = UdpDestPortList;
944   UdpDestPortList = new;
945
946   // And then we're done
947   return 0;
948 }
949
950 /* -------------------------------------------------------------------------
951  * Function   : get_ipv4_str
952  * Description: Convert the specified address to an IPv4 compatible string
953  * Input      : address - IPv4 address to convert to string
954  *              s       - string buffer to contain the resulting string
955  *              maxlen  - maximum length of the string buffer
956  * Output     : none
957  * Return     : Pointer to the string buffer containing the result
958  * Data Used  : none
959  * ------------------------------------------------------------------------- */
960 char *
961 get_ipv4_str(uint32_t address, char *s, size_t maxlen)
962 {
963   struct sockaddr_in v4;
964
965   v4.sin_addr.s_addr = address;
966   inet_ntop(AF_INET, &v4.sin_addr, s, maxlen);
967
968   return s;
969 }
970
971 /* -------------------------------------------------------------------------
972  * Function   : get_ipv6_str
973  * Description: Convert the specified address to an IPv4 compatible string
974  * Input      : address - IPv6 address to convert to string
975  *              s       - string buffer to contain the resulting string
976  *              maxlen  - maximum length of the string buffer
977  * Output     : none
978  * Return     : Pointer to the string buffer containing the result
979  * Data Used  : none
980  * ------------------------------------------------------------------------- */
981 char *
982 get_ipv6_str(unsigned char* address, char *s, size_t maxlen)
983 {
984   struct sockaddr_in6 v6;
985
986   memcpy(v6.sin6_addr.s6_addr, address, sizeof(v6.sin6_addr.s6_addr));
987   inet_ntop(AF_INET6, &v6.sin6_addr, s, maxlen);
988
989   return s;
990 }
991
992 /* -------------------------------------------------------------------------
993  * Function   : is_broadcast
994  * Description: Check whether the address represents a broadcast address
995  * Input      : addr - IPv4 address to check
996  * Output     : none
997  * Return     : true if broadcast address, false otherwise
998  * Data Used  : none
999  * ------------------------------------------------------------------------- */
1000 bool
1001 is_broadcast(const struct sockaddr_in addr)
1002 {
1003   if (addr.sin_addr.s_addr == 0xFFFFFFFF)
1004     return true;
1005
1006   return false;
1007 }
1008
1009 /* -------------------------------------------------------------------------
1010  * Function   : is_multicast
1011  * Description: Check whether the address represents a multicast address
1012  * Input      : addr - IPv4 address to check
1013  * Output     : none
1014  * Return     : true if broadcast address, false otherwise
1015  * Data Used  : none
1016  * ------------------------------------------------------------------------- */
1017 bool
1018 is_multicast(const struct sockaddr_in addr)
1019 {
1020   if ((htonl(addr.sin_addr.s_addr) & 0xE0000000) == 0xE0000000)
1021     return true;
1022
1023   return false;
1024 }
1025
1026 #ifdef INCLUDE_DEBUG_OUTPUT
1027 /* -------------------------------------------------------------------------
1028  * Function   : dump_packet
1029  * Description: Dump the specified data as hex output
1030  * Input      : packet - packet to dump to output
1031  *              length - length of the data in the packet
1032  * Output     : none
1033  * Return     : nothing
1034  * Data Used  : none
1035  * ------------------------------------------------------------------------- */
1036 void
1037 dump_packet(unsigned char* packet, int length)
1038 {
1039   int idx;
1040
1041   OLSR_PRINTF(1, "%s: ", PLUGIN_NAME_SHORT);
1042   for (idx = 0; idx < length; idx++) {
1043     if (idx > 0 && ((idx % 16) == 0))
1044       OLSR_PRINTF(1, "\n%s: ", PLUGIN_NAME_SHORT);
1045     OLSR_PRINTF(1, "%2.2X ", packet[idx]);
1046   }
1047   OLSR_PRINTF(1, "\n");
1048 }
1049 #endif
1050
1051 /* -------------------------------------------------------------------------
1052  * Function   : check_and_mark_recent_packet
1053  * Description: Wrapper function for the Hash based duplicate check
1054  * Input      : data - pointer to a packet of data to be checked
1055  * Output     : none
1056  * Return     : true if duplicate packet, false otherwise
1057  * Data Used  : P2pdUseHash
1058  * ------------------------------------------------------------------------- */
1059 bool
1060 check_and_mark_recent_packet(unsigned char *data,
1061                              int len __attribute__ ((unused)))
1062 {
1063   unsigned char * ipPacket;
1064   uint16_t ipPacketLen;
1065   uint32_t crc32;
1066
1067   /* If we don't use this filter bail out here */
1068   if (!P2pdUseHash)
1069     return false;
1070     
1071   /* Clean up the hash table each time before we check it */
1072   PrunePacketHistory(NULL);
1073
1074   /* Check for duplicate IP packets now based on a hash */
1075   ipPacket = GetIpPacket(data);
1076   ipPacketLen = GetIpTotalLength(ipPacket);
1077
1078   /* Calculate packet fingerprint */
1079   crc32 = PacketCrc32(ipPacket, ipPacketLen);
1080
1081   /* Check if this packet was seen recently */
1082   if (CheckAndMarkRecentPacket(crc32))
1083   {
1084     OLSR_PRINTF(
1085       8,
1086       "%s: --> discarding: packet is duplicate\n",
1087       PLUGIN_NAME_SHORT);
1088     return true;
1089   }
1090
1091   return false;
1092 }