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