lib/p2pd/src/p2pd.c

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