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