lib/p2pd/src/p2pd.c

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