Add p2dp plugin (generalized mdns), thanks to Teco Boot <teco@inf-net.nl>
[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 "log.h"                /* Teco: syslog */
75 #include "parser.h"
76
77 /* plugin includes */
78 #include "NetworkInterfaces.h"  /* NonOlsrInterface, CreateBmfNetworkInterfaces(), CloseBmfNetworkInterfaces() */
79 //#include "Address.h"            /* IsMulticast() */
80 #include "Packet.h"             /* ENCAP_HDR_LEN, BMF_ENCAP_TYPE, BMF_ENCAP_LEN etc. */
81 #include "dllist.h"
82
83 int P2pdTtl=0;
84 int P2pdDuplicateTimeout           = P2PD_VALID_TIME;
85
86 /* List of UDP destination address and port information */
87 struct UdpDestPort *                 UdpDestPortList = NULL;
88
89 /* List of filter entries to check for duplicate messages
90  */
91 struct node *                        dupFilterHead = NULL;
92 struct node *                        dupFilterTail = NULL;
93
94 clockid_t clockid = CLOCK_MONOTONIC;
95
96 bool is_broadcast(const struct sockaddr_in addr);
97 bool is_multicast(const struct sockaddr_in addr);
98
99 /* -------------------------------------------------------------------------
100  * Function   : PacketReceivedFromOLSR
101  * Description: Handle a received packet from a OLSR message
102  * Input      : ipPacket into an unsigned char and the lenght of the packet
103  * Output     : none
104  * Return     : none
105  * Data Used  : BmfInterfaces
106  * ------------------------------------------------------------------------- */
107 static void
108 PacketReceivedFromOLSR(unsigned char *encapsulationUdpData, int len)
109 {
110   struct ip *ipHeader;        /* IP header inside the encapsulated IP packet */
111   struct ip6_hdr *ip6Header;  /* IP header inside the encapsulated IP packet */
112   struct NonOlsrInterface *walker;
113   int stripped_len = 0;
114   ipHeader = (struct ip *)encapsulationUdpData;
115   ip6Header = (struct ip6_hdr *)encapsulationUdpData;
116   //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN got packet from OLSR message\n");
117
118   /* Check with each network interface what needs to be done on it */
119   for (walker = nonOlsrInterfaces; walker != NULL; walker = walker->next) {
120     /* To a non-OLSR interface: unpack the encapsulated IP packet and forward it */
121     if (walker->olsrIntf == NULL) {
122       int nBytesWritten;
123       struct sockaddr_ll dest;
124
125       memset(&dest, 0, sizeof(dest));
126       dest.sll_family = AF_PACKET;
127       if ((encapsulationUdpData[0] & 0xf0) == 0x40) {
128         dest.sll_protocol = htons(ETH_P_IP);
129         stripped_len = ntohs(ipHeader->ip_len);
130       }
131       
132       if ((encapsulationUdpData[0] & 0xf0) == 0x60) {
133         dest.sll_protocol = htons(ETH_P_IPV6);
134         stripped_len = 40 + ntohs(ip6Header->ip6_plen); //IPv6 Header size (40) + payload_len 
135       }
136       
137       // Sven-Ola: Don't know how to handle the "stripped_len is uninitialized" condition, maybe exit(1) is better...?
138       if (0 == stripped_len)
139         return;
140       
141       //TODO: if packet is not IP die here
142       
143       if (stripped_len > len) {
144       }
145       
146       dest.sll_ifindex = if_nametoindex(walker->ifName);
147       dest.sll_halen = IFHWADDRLEN;
148
149       if (olsr_cnf->ip_version == AF_INET) {
150         /* Use all-ones as destination MAC address. When the IP destination is
151          * a multicast address, the destination MAC address should normally also
152          * be a multicast address. E.g., when the destination IP is 224.0.0.1,
153          * the destination MAC should be 01:00:5e:00:00:01. However, it does not
154          * seem to matter when the destination MAC address is set to all-ones
155          * in that case. */
156
157         if (IsMulticastv4(ipHeader)) {
158           dest.sll_addr[0] = 0x01;
159           dest.sll_addr[1] = 0x00;
160           dest.sll_addr[2] = 0x5E;
161           dest.sll_addr[3] = (ipHeader->ip_dst.s_addr >> 16) & 0xFF;
162           dest.sll_addr[4] = (ipHeader->ip_dst.s_addr >> 8) & 0xFF;
163           dest.sll_addr[5] = ipHeader->ip_dst.s_addr & 0xFF;
164         } else /* if (IsBroadcast(ipHeader)) */ {
165           memset(dest.sll_addr, 0xFF, IFHWADDRLEN);
166         }
167       } else /*(olsr_cnf->ip_version == AF_INET6) */ {
168         if (IsMulticastv6(ip6Header)) {
169           dest.sll_addr[0] = 0x33;
170           dest.sll_addr[1] = 0x33;
171           dest.sll_addr[2] = ip6Header->ip6_dst.s6_addr[12];
172           dest.sll_addr[3] = ip6Header->ip6_dst.s6_addr[13];
173           dest.sll_addr[4] = ip6Header->ip6_dst.s6_addr[14];
174           dest.sll_addr[5] = ip6Header->ip6_dst.s6_addr[15];
175         }
176       }
177
178       nBytesWritten = sendto(walker->capturingSkfd,
179                              encapsulationUdpData,
180                              stripped_len,
181                              0,
182                              (struct sockaddr *)&dest,
183                              sizeof(dest));
184       if (nBytesWritten != stripped_len) {
185         P2pdPError("sendto() error forwarding unpacked encapsulated pkt on \"%s\"", walker->ifName);
186       } else {
187
188         //OLSR_PRINTF(
189         //  2,
190         //  "%s: --> unpacked and forwarded on \"%s\"\n",
191         //  PLUGIN_NAME_SHORT,
192         //  walker->ifName);
193       }
194     }                           /* if (walker->olsrIntf == NULL) */
195   }
196 }                               /* PacketReceivedFromOLSR */
197
198 /* Highest-numbered open socket file descriptor. To be used as first
199  * parameter in calls to select(...). */
200 int HighestSkfd = -1;
201
202 /* Set of socket file descriptors */
203 fd_set InputSet;
204
205 bool
206 p2pd_message_seen(struct node **head, struct node **tail, union olsr_message *m)
207 {
208   struct node * curr;
209   time_t now;
210
211   now = time(NULL);
212   
213   // Check whether any entries have aged
214   curr = *head;
215   while (curr) {
216     struct DupFilterEntry *filter;
217     struct node * next = curr->next; // Save the current pointer since curr may be destroyed
218     
219     filter = (struct DupFilterEntry*)curr->data;
220     
221     if ((filter->creationtime + P2pdDuplicateTimeout) < now)
222       remove_node(head, tail, curr, true);
223       
224     // Skip to the next element
225     curr = next;
226   }
227   
228   // Now check whether there are any duplicates
229   for (curr = *head; curr; curr = curr->next) {
230     struct DupFilterEntry *filter = (struct DupFilterEntry*)curr->data;
231     
232     if (olsr_cnf->ip_version == AF_INET) {
233       if (filter->address.v4.s_addr  == m->v4.originator &&
234           filter->msgtype            == m->v4.olsr_msgtype &&
235           filter->seqno              == m->v4.seqno) {
236           return true;
237       }
238     } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
239       if (memcmp(filter->address.v6.s6_addr,
240                  m->v6.originator.s6_addr,
241                  sizeof(m->v6.originator.s6_addr)) == 0 &&
242           filter->msgtype            == m->v6.olsr_msgtype &&
243           filter->seqno              == m->v6.seqno) {
244           return true;
245       }
246     }
247   }
248   
249   return false;
250 }
251
252 void
253 p2pd_store_message(struct node **head, struct node **tail, union olsr_message *m)
254 {
255   time_t now;
256
257   // Store a message into the database
258   struct DupFilterEntry *new_dup = calloc(1, sizeof(struct DupFilterEntry));
259   if (new_dup == NULL) {
260     olsr_printf(1, "P2PD: Out of memory\n");
261     return;
262   }
263
264   now = time(NULL);
265   
266   new_dup->creationtime = now;
267   if (olsr_cnf->ip_version == AF_INET) {
268     new_dup->address.v4.s_addr = m->v4.originator;
269     new_dup->msgtype = m->v4.olsr_msgtype;
270     new_dup->seqno = m->v4.seqno;
271   } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
272     memcpy(new_dup->address.v6.s6_addr,
273            m->v6.originator.s6_addr,
274            sizeof(m->v6.originator.s6_addr));
275     new_dup->msgtype = m->v6.olsr_msgtype;
276     new_dup->seqno = m->v6.seqno;
277   }
278   
279   // Add the element to the head of the list
280   append_node(head, tail, new_dup);
281 }
282
283 bool
284 p2pd_is_duplicate_message(union olsr_message *msg)
285 {
286   if(p2pd_message_seen(&dupFilterHead, &dupFilterTail, msg)) {
287     return true;
288   }
289
290   p2pd_store_message(&dupFilterHead, &dupFilterTail, msg);
291   
292   return false;
293 }
294
295 bool
296 olsr_parser(union olsr_message *m, struct interface *in_if __attribute__ ((unused)), union olsr_ip_addr *ipaddr __attribute__ ((unused)))
297 {
298   union olsr_ip_addr originator;
299   int size;
300   uint32_t vtime;
301
302   //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN: Received msg in parser\n");
303   
304         /* Fetch the originator of the messsage */
305   if (olsr_cnf->ip_version == AF_INET) {
306     memcpy(&originator, &m->v4.originator, olsr_cnf->ipsize);
307     vtime = me_to_reltime(m->v4.olsr_vtime);
308     size = ntohs(m->v4.olsr_msgsize);
309   } else {
310     memcpy(&originator, &m->v6.originator, olsr_cnf->ipsize);
311     vtime = me_to_reltime(m->v6.olsr_vtime);
312     size = ntohs(m->v6.olsr_msgsize);
313   }
314
315   /* Check if message originated from this node.
316    *         If so - back off */
317   if (ipequal(&originator, &olsr_cnf->main_addr))
318     return false;          /* Don't forward either */
319
320   /* Check for duplicates for processing */
321   if (p2pd_is_duplicate_message(m))
322     return true;  /* Don't process but allow to be forwarded */
323
324   if (olsr_cnf->ip_version == AF_INET) {
325     PacketReceivedFromOLSR((unsigned char *)&m->v4.message, size - 12);
326   } else {
327     PacketReceivedFromOLSR((unsigned char *)&m->v6.message, size - 12 - 96);
328   }
329
330         return true;
331 }
332
333 //Sends a packet in the OLSR network
334 void
335 olsr_p2pd_gen(unsigned char *packet, int len)
336 {
337   /* send buffer: huge */
338   char buffer[10240];
339   int aligned_size;
340   union olsr_message *message = (union olsr_message *)buffer;
341   struct interface *ifn;
342   
343   aligned_size=len;
344
345   if ((aligned_size % 4) != 0) {
346     aligned_size = (aligned_size - (aligned_size % 4)) + 4;
347   }
348
349   /* fill message */
350   if (olsr_cnf->ip_version == AF_INET) {
351     /* IPv4 */
352     message->v4.olsr_msgtype  = P2PD_MESSAGE_TYPE;
353     message->v4.olsr_vtime    = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
354     memcpy(&message->v4.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
355     message->v4.ttl           = P2pdTtl ? P2pdTtl : MAX_TTL;
356     message->v4.hopcnt        = 0;
357     message->v4.seqno         = htons(get_msg_seqno());
358     message->v4.olsr_msgsize  = htons(aligned_size + 12);
359     memset(&message->v4.message, 0, aligned_size);
360     memcpy(&message->v4.message, packet, len);
361     aligned_size = aligned_size + 12;
362   } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
363     /* IPv6 */
364     message->v6.olsr_msgtype  = P2PD_MESSAGE_TYPE;
365     message->v6.olsr_vtime    = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
366     memcpy(&message->v6.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
367     message->v6.ttl           = P2pdTtl ? P2pdTtl : MAX_TTL;
368     message->v6.hopcnt        = 0;
369     message->v6.seqno         = htons(get_msg_seqno());
370     message->v6.olsr_msgsize  = htons(aligned_size + 12 + 96);
371     memset(&message->v6.message, 0, aligned_size);
372     memcpy(&message->v6.message, packet, len);
373     aligned_size = aligned_size + 12 + 96;
374   }
375
376   /* looping trough interfaces */
377   for (ifn = ifnet; ifn; ifn = ifn->int_next) {
378     //OLSR_PRINTF(1, "P2PD PLUGIN: Generating packet - [%s]\n", ifn->int_name);
379
380     if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
381       /* send data and try again */
382       net_output(ifn);
383       if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
384         //OLSR_PRINTF(1, "P2PD PLUGIN: could not send on interface: %s\n", ifn->int_name);
385       }
386     }
387   }
388 }
389
390 /* -------------------------------------------------------------------------
391  * Function   : P2pdPError
392  * Description: Prints an error message at OLSR debug level 1.
393  *              First the plug-in name is printed. Then (if format is not NULL
394  *              and *format is not empty) the arguments are printed, followed
395  *              by a colon and a blank. Then the message and a new-line.
396  * Input      : format, arguments
397  * Output     : none
398  * Return     : none
399  * Data Used  : none
400  * ------------------------------------------------------------------------- */
401 void
402 P2pdPError(const char *format, ...)
403 {
404 #define MAX_STR_DESC 255
405   char strDesc[MAX_STR_DESC];
406
407 #if !defined REMOVE_LOG_DEBUG
408   char *stringErr = strerror(errno);
409 #endif
410
411   /* Rely on short-circuit boolean evaluation */
412   if (format == NULL || *format == '\0') {
413     //OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", PLUGIN_NAME, stringErr);
414   } else {
415     va_list arglist;
416
417     va_start(arglist, format);
418     vsnprintf(strDesc, MAX_STR_DESC, format, arglist);
419     va_end(arglist);
420
421     strDesc[MAX_STR_DESC - 1] = '\0';   /* Ensures null termination */
422     
423 #if !defined REMOVE_LOG_DEBUG
424     OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", strDesc, stringErr);
425 #endif
426   }
427 }                               /* P2pdPError */
428
429 /* -------------------------------------------------------------------------
430  * Function   : MainAddressOf
431  * Description: Lookup the main address of a node
432  * Input      : ip - IP address of the node
433  * Output     : none
434  * Return     : The main IP address of the node
435  * Data Used  : none
436  * ------------------------------------------------------------------------- */
437 union olsr_ip_addr *
438 MainAddressOf(union olsr_ip_addr *ip)
439 {
440   union olsr_ip_addr *result;
441
442   /* TODO: mid_lookup_main_addr() is not thread-safe! */
443   result = mid_lookup_main_addr(ip);
444   if (result == NULL) {
445     result = ip;
446   }
447   return result;
448 }                               /* MainAddressOf */
449
450
451 /* -------------------------------------------------------------------------
452  * Function   : InUdpDestPortList
453  * Description: Check whether the specified address and port is in the list of
454  *              configured UDP destination/port entries
455  * Input      : ip_version  - IP version to use for this check
456  *              addr        - address to check for in the list
457  *              port        - port to check for in the list
458  * Output     : none
459  * Return     : true if destination/port combination was found, false otherwise
460  * Data Used  : UdpDestPortList
461  * ------------------------------------------------------------------------- */
462 bool
463 InUdpDestPortList(int ip_version, union olsr_ip_addr *addr, uint16_t port)
464 {
465   struct UdpDestPort *walker;
466   
467   for (walker = UdpDestPortList; walker; walker = walker->next) {
468     if (walker->ip_version == ip_version) {
469       if (ip_version == AF_INET) {
470         if (addr->v4.s_addr == walker->address.v4.s_addr &&
471             walker->port == port)
472           return true;  // Found so we can stop here
473       } else /* ip_version == AF_INET6 */ {
474         if (memcmp(addr->v6.s6_addr,
475                    walker->address.v6.s6_addr,
476                    sizeof(addr->v6.s6_addr) == 0) &&
477             walker->port == port)
478           return true;  // Found so we can stop here
479       }
480     }
481   }
482   return false;
483 }
484
485 /* -------------------------------------------------------------------------
486  * Function   : P2pdPacketCaptured
487  * Description: Handle a captured IP packet
488  * Input      : encapsulationUdpData - space for the encapsulation header, 
489  *                                                      followed by the captured IP packet
490  *                                                      nBytes - The number of bytes in the data packet 
491  * Output     : none
492  * Return     : none
493  * Data Used  : P2pdInterfaces
494  * Notes      : The IP packet is assumed to be captured on a socket of family
495  *              PF_PACKET and type SOCK_DGRAM (cooked).
496  * ------------------------------------------------------------------------- */
497 static void
498 P2pdPacketCaptured(unsigned char *encapsulationUdpData, int nBytes)
499 {
500   union olsr_ip_addr src;      /* Source IP address in captured packet */
501   union olsr_ip_addr dst;      /* Destination IP address in captured packet */
502   union olsr_ip_addr *origIp;  /* Main OLSR address of source of captured packet */
503   struct ip *ipHeader;         /* The IP header inside the captured IP packet */
504   struct ip6_hdr *ipHeader6;   /* The IP header inside the captured IP packet */
505   struct udphdr *udpHeader;
506   u_int16_t destPort;
507
508   if ((encapsulationUdpData[0] & 0xf0) == 0x40) {       //IPV4
509
510     ipHeader = (struct ip *)encapsulationUdpData;
511
512     dst.v4 = ipHeader->ip_dst;
513
514     if (ipHeader->ip_p != SOL_UDP) {
515       /* Not UDP */
516       //OLSR_PRINTF(1,"NON UDP PACKET\n");
517       return;                   /* for */
518     }
519
520     // If we're dealing with a fragment we bail out here since there's no valid
521     // UDP header in this message
522     if (IsIpv4Fragment(ipHeader)) {
523       return;
524     }
525     
526     udpHeader = (struct udphdr *)(encapsulationUdpData +
527                                   GetIpHeaderLength(encapsulationUdpData));
528     destPort = ntohs(udpHeader->dest);
529     
530     if (!InUdpDestPortList(AF_INET, &dst, destPort))
531        return;
532   }                            //END IPV4
533   else if ((encapsulationUdpData[0] & 0xf0) == 0x60) {  //IPv6
534
535     ipHeader6 = (struct ip6_hdr *)encapsulationUdpData;
536     
537     memcpy(&dst.v6, &ipHeader6->ip6_dst, sizeof(struct in6_addr));
538     
539     if (ipHeader6->ip6_dst.s6_addr[0] == 0xff)  //Multicast
540     {
541       //Continue
542     } else {
543       return;                   //not multicast
544     }
545     if (ipHeader6->ip6_nxt != SOL_UDP) {
546       /* Not UDP */
547       //OLSR_PRINTF(1,"NON UDP PACKET\n");
548       return;                   /* for */
549     }
550     
551     // Check whether this is a IPv6 fragment
552     if (IsIpv6Fragment(ipHeader6)) {
553       return;
554     }
555     
556     udpHeader = (struct udphdr *)(encapsulationUdpData + 40);
557     destPort = ntohs(udpHeader->dest);
558
559     if (!InUdpDestPortList(AF_INET6, &dst, destPort))
560       return;
561   }                             //END IPV6
562   else {
563     return;                     //Is not IP packet
564   }
565
566   /* Lookup main address of source in the MID table of OLSR */
567   origIp = MainAddressOf(&src);
568
569   // send the packet to OLSR forward mechanism
570   olsr_p2pd_gen(encapsulationUdpData, nBytes);
571 }                               /* P2pdPacketCaptured */
572
573
574 /* -------------------------------------------------------------------------
575  * Function   : DoP2pd
576  * Description: This function is registered with the OLSR scheduler and called when something is captured
577  * Input      : none
578  * Output     : none
579  * Return     : none
580  * Data Used  :
581  * ------------------------------------------------------------------------- */
582 void
583 DoP2pd(int skfd, void *data __attribute__ ((unused)), unsigned int flags __attribute__ ((unused)))
584 {
585   unsigned char rxBuffer[P2PD_BUFFER_SIZE];
586   if (skfd >= 0) {
587     struct sockaddr_ll pktAddr;
588     socklen_t addrLen = sizeof(pktAddr);
589     int nBytes;
590     unsigned char *ipPacket;
591
592     /* Receive the captured Ethernet frame, leaving space for the BMF
593      * encapsulation header */
594     ipPacket = GetIpPacket(rxBuffer);
595     nBytes = recvfrom(skfd, ipPacket, P2PD_BUFFER_SIZE,  //TODO: understand how to change this
596                       0, (struct sockaddr *)&pktAddr, &addrLen);
597     if (nBytes < 0) {
598
599       return;                   /* for */
600     }
601
602     /* if (nBytes < 0) */
603     /* Check if the number of received bytes is large enough for an IP
604      * packet which contains at least a minimum-size IP header.
605      * Note: There is an apparent bug in the packet socket implementation in
606      * combination with VLAN interfaces. On a VLAN interface, the value returned
607      * by 'recvfrom' may (but need not) be 4 (bytes) larger than the value
608      * returned on a non-VLAN interface, for the same ethernet frame. */
609     if (nBytes < (int)sizeof(struct ip)) {
610       ////OLSR_PRINTF(
611       //              1,
612       //              "%s: captured frame too short (%d bytes) on \"%s\"\n",
613       //              PLUGIN_NAME,
614       //              nBytes,
615       //              walker->ifName);
616
617       return;                   /* for */
618     }
619
620     if (pktAddr.sll_pkttype == PACKET_OUTGOING ||
621         pktAddr.sll_pkttype == PACKET_MULTICAST ||
622         pktAddr.sll_pkttype == PACKET_BROADCAST) {
623       /* A multicast or broadcast packet was captured */
624
625       P2pdPacketCaptured(ipPacket, nBytes);
626
627     }                           /* if (pktAddr.sll_pkttype == ...) */
628   }                             /* if (skfd >= 0 && (FD_ISSET...)) */
629 }                               /* DoP2pd */
630
631 /* -------------------------------------------------------------------------
632  * Function   : InitP2pd
633  * Description: Initialize the P2pd plugin
634  * Input      : skipThisInterface - pointer to interface to skip
635  * Output     : none
636  * Return     : Always 0
637  * Data Used  : none
638  * ------------------------------------------------------------------------- */
639 int
640 InitP2pd(struct interface *skipThisIntf)
641 {
642   //Tells OLSR to launch olsr_parser when the packets for this plugin arrive
643   //olsr_parser_add_function(&olsr_parser, PARSER_TYPE,1);
644   olsr_parser_add_function(&olsr_parser, PARSER_TYPE);
645   
646   //Creates captures sockets and register them to the OLSR scheduler
647   CreateNonOlsrNetworkInterfaces(skipThisIntf);
648
649   return 0;
650 }                               /* InitP2pd */
651
652 /* -------------------------------------------------------------------------
653  * Function   : CloseP2pd
654  * Description: Close the P2pd plugin and clean up
655  * Input      : none
656  * Output     : none
657  * Return     : none
658  * Data Used  :
659  * ------------------------------------------------------------------------- */
660 void
661 CloseP2pd(void)
662 {
663   CloseNonOlsrNetworkInterfaces();
664 }
665
666 /* -------------------------------------------------------------------------
667  * Function   : SetP2pdTtl
668  * Description: Set the TTL for message from this plugin
669  * Input      : value - parameter value to evaluate
670  * Output     : none
671  * Return     : Always 0
672  * Data Used  : P2pdTtl
673  * ------------------------------------------------------------------------- */
674 int
675 SetP2pdTtl(const char *value, void *data __attribute__ ((unused)), set_plugin_parameter_addon addon __attribute__ ((unused)))
676 {
677   assert(value != NULL);
678   P2pdTtl = atoi(value);
679
680   return 0;
681 }
682
683 bool is_broadcast(const struct sockaddr_in addr)
684 {
685   if (addr.sin_addr.s_addr == 0xFFFFFFFF)
686     return true;
687     
688   return false;
689 }
690
691 bool is_multicast(const struct sockaddr_in addr)
692 {
693   if ((htonl(addr.sin_addr.s_addr) & 0xE0000000) == 0xE0000000)
694     return true;
695
696   return false;
697 }
698
699 /* -------------------------------------------------------------------------
700  * Function   : AddUdpDestPort
701  * Description: Set the UDP destination/port combination as an entry in the
702  *              UdpDestPortList
703  * Input      : value - parameter value to evaluate
704  * Output     : none
705  * Return     : -1 on error condition, 0 if all is ok
706  * Data Used  : UdpDestPortList
707  * ------------------------------------------------------------------------- */
708 int
709 AddUdpDestPort(const char *value, void *data __attribute__ ((unused)), set_plugin_parameter_addon addon __attribute__ ((unused)))
710 {
711   char destAddr[INET6_ADDRSTRLEN];
712   uint16_t destPort;
713   int num;
714   struct UdpDestPort *    new;
715   struct sockaddr_in      addr4;
716   struct sockaddr_in6     addr6;
717   int                     ip_version    = AF_INET;
718   int                     res;
719   
720   assert(value != NULL);
721   
722   // Retrieve the data from the argument string passed
723   memset(destAddr, 0, sizeof(destAddr));
724   num = sscanf(value, "%45s %hd", destAddr, &destPort);
725   if (num != 2) {
726     olsr_printf(1, "Invalid argument for \"UdpDestPort\"");
727     return -1;
728   }
729         
730   // Check whether we're dealing with an IPv4 or IPv6 address
731   // When the string contains a ':' we can assume we're dealing with IPv6
732   if (strchr(destAddr, (int)':')) {
733     ip_version = AF_INET6;
734   }
735
736   // Check whether the specified address was either IPv4 multicast,
737   // IPv4 broadcast or IPv6 multicast.
738
739   switch (ip_version) {
740   case AF_INET:
741     res = inet_pton(AF_INET, destAddr, &addr4.sin_addr);
742     if (!is_broadcast(addr4) && !is_multicast(addr4)) {
743       olsr_printf(1, "WARNING: IPv4 address must be multicast or broadcast... ");
744     }
745     break;
746   case AF_INET6:
747     res = inet_pton(AF_INET6, destAddr, &addr6.sin6_addr);
748     if (addr6.sin6_addr.s6_addr[0] != 0xFF) {
749       olsr_printf(1, "WARNING: IPv6 address must be multicast... ");
750       return -1;
751     }
752     break;
753   }
754   // Determine whether it is a valid IP address
755   if (res == 0) {
756     olsr_printf(1, "Invalid address specified for \"UdpDestPort\"");
757     return -1;
758   }
759         
760   // Create a new entry and link it into the chain
761   new = calloc(1, sizeof(struct UdpDestPort));
762   if (new == NULL) {
763     olsr_printf(1, "P2PD: Out of memory");
764     return -1;
765   }
766         
767   new->ip_version = ip_version;
768   switch (ip_version) {
769   case AF_INET:
770     new->address.v4.s_addr = addr4.sin_addr.s_addr;
771     break;
772   case AF_INET6:
773     memcpy(&new->address.v6.s6_addr, &addr6.sin6_addr.s6_addr, sizeof(sizeof(addr6.sin6_addr.s6_addr)));
774     break;
775   }
776   new->port = destPort;
777   new->next = UdpDestPortList;
778   UdpDestPortList = new;
779         
780   // And then we're done
781   return 0;
782 }
783