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