lib/p2pd/src/p2pd.c

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