2 * The olsr.org Optimized Link-State Routing daemon(olsrd)
3 * Copyright (c) 2004-2009, the olsr.org team - see HISTORY file
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
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
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.
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.
33 * Visit http://www.olsr.org for more information.
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.
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() */
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() */
60 #include <netinet/in.h>
61 #include <netinet/ip6.h>
66 #include "plugin_util.h" /* set_plugin_int */
67 #include "defs.h" /* olsr_cnf, //OLSR_PRINTF */
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 */
76 #include "NetworkInterfaces.h" /* NonOlsrInterface,
77 CreateBmfNetworkInterfaces(),
78 CloseBmfNetworkInterfaces() */
79 //#include "Address.h" /* IsMulticast() */
80 #include "Packet.h" /* ENCAP_HDR_LEN,
83 #include "PacketHistory.h"
87 int P2pdUseHash = 0; /* Switch off hash filter by default */
88 int P2pdDuplicateTimeout = P2PD_VALID_TIME;
90 /* List of UDP destination address and port information */
91 struct UdpDestPort * UdpDestPortList = NULL;
93 /* List of filter entries to check for duplicate messages
95 struct node * dupFilterHead = NULL;
96 struct node * dupFilterTail = NULL;
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);
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
113 * Data Used : BmfInterfaces
114 * ------------------------------------------------------------------------- */
116 PacketReceivedFromOLSR(unsigned char *encapsulationUdpData, int len)
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;
125 bool isInList = false;
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");
131 if (check_and_mark_recent_packet(encapsulationUdpData, len))
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) {
139 struct sockaddr_ll dest;
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);
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)
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)
159 //TODO: if packet is not IP die here
161 if (stripped_len > len) {
164 dest.sll_ifindex = if_nametoindex(walker->ifName);
165 dest.sll_halen = IFHWADDRLEN;
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
176 if (IsMulticastv4(ipHeader)) {
177 in_addr_t addr = ntohl(ipHeader->ip_dst.s_addr);
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;
186 /* broadcast or whatever */
187 memset(dest.sll_addr, 0xFF, IFHWADDRLEN);
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];
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
212 "%s: Not in dest/port list: %s:%d\n",
214 get_ipv4_str(destAddr.v4.s_addr,
219 #endif /* INCLUDE_DEBUG_OUTPUT */
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
231 "%s: Not in dest/port list: %s:%d\n",
233 get_ipv6_str(destAddr.v6.s6_addr,
238 #endif /* INCLUDE_DEBUG_OUTPUT */
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.
250 nBytesWritten = sendto(walker->capturingSkfd,
251 encapsulationUdpData,
254 (struct sockaddr *)&dest,
256 if (nBytesWritten != stripped_len) {
257 P2pdPError("sendto() error forwarding unpacked encapsulated pkt on \"%s\"",
263 // "%s: --> unpacked and forwarded on \"%s\"\n",
264 // PLUGIN_NAME_SHORT,
267 } /* if (walker->olsrIntf == NULL) */
269 } /* PacketReceivedFromOLSR */
271 /* Highest-numbered open socket file descriptor. To be used as first
272 * parameter in calls to select(...).
274 int HighestSkfd = -1;
276 /* Set of socket file descriptors */
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
286 * Return : true if message was found, false otherwise
287 * Data Used : P2pdDuplicateTimeout
288 * ------------------------------------------------------------------------- */
290 p2pd_message_seen(struct node **head, struct node **tail, union olsr_message *m)
297 // Check whether any entries have aged
300 struct DupFilterEntry *filter;
301 struct node * next = curr->next; // Save the current pointer since curr may
304 filter = (struct DupFilterEntry*)curr->data;
306 if ((filter->creationtime + P2pdDuplicateTimeout) < now)
307 remove_node(head, tail, curr, true);
309 // Skip to the next element
313 // Now check whether there are any duplicates
314 for (curr = *head; curr; curr = curr->next) {
315 struct DupFilterEntry *filter = (struct DupFilterEntry*)curr->data;
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) {
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) {
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
346 * ------------------------------------------------------------------------- */
348 p2pd_store_message(struct node **head, struct node **tail, union olsr_message *m)
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);
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;
374 // Add the element to the head of the list
375 append_node(head, tail, new_dup);
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
383 * Return : true if message was found, false otherwise
385 * ------------------------------------------------------------------------- */
387 p2pd_is_duplicate_message(union olsr_message *msg)
389 if(p2pd_message_seen(&dupFilterHead, &dupFilterTail, msg)) {
393 p2pd_store_message(&dupFilterHead, &dupFilterTail, msg);
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)
406 * Return : false if message should be supressed, true otherwise
408 * ------------------------------------------------------------------------- */
410 olsr_parser(union olsr_message *m,
411 struct interface *in_if __attribute__ ((unused)),
412 union olsr_ip_addr *ipaddr __attribute__ ((unused)))
414 union olsr_ip_addr originator;
417 //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN: Received msg in parser\n");
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);
424 memcpy(&originator, &m->v6.originator, olsr_cnf->ipsize);
425 size = ntohs(m->v6.olsr_msgsize);
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 */
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 */
437 if (olsr_cnf->ip_version == AF_INET) {
438 PacketReceivedFromOLSR((unsigned char *)&m->v4.message, size - 12);
440 PacketReceivedFromOLSR((unsigned char *)&m->v6.message, size - 12 - 96);
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
454 * ------------------------------------------------------------------------- */
456 olsr_p2pd_gen(unsigned char *packet, int len)
458 /* send buffer: huge */
461 union olsr_message *message = (union olsr_message *)buffer;
462 struct interface *ifn;
466 if ((aligned_size % 4) != 0) {
467 aligned_size = (aligned_size - (aligned_size % 4)) + 4;
471 if (olsr_cnf->ip_version == AF_INET) {
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) */ {
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;
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);
501 if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
502 /* send data and try again */
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);
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
521 * ------------------------------------------------------------------------- */
523 P2pdPError(const char *format, ...)
525 #define MAX_STR_DESC 255
526 char strDesc[MAX_STR_DESC];
528 #if !defined REMOVE_LOG_DEBUG
529 char *stringErr = strerror(errno);
530 #endif /* !defined REMOVE_LOG_DEBUG */
532 /* Rely on short-circuit boolean evaluation */
533 if (format == NULL || *format == '\0') {
534 //OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", PLUGIN_NAME, stringErr);
538 va_start(arglist, format);
539 vsnprintf(strDesc, MAX_STR_DESC, format, arglist);
542 strDesc[MAX_STR_DESC - 1] = '\0'; /* Ensures null termination */
544 #if !defined REMOVE_LOG_DEBUG
545 OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", strDesc, stringErr);
546 #endif /* !defined REMOVE_LOG_DEBUG */
550 /* -------------------------------------------------------------------------
551 * Function : MainAddressOf
552 * Description: Lookup the main address of a node
553 * Input : ip - IP address of the node
555 * Return : The main IP address of the node
557 * ------------------------------------------------------------------------- */
559 MainAddressOf(union olsr_ip_addr *ip)
561 union olsr_ip_addr *result;
563 /* TODO: mid_lookup_main_addr() is not thread-safe! */
564 result = mid_lookup_main_addr(ip);
565 if (result == NULL) {
569 } /* MainAddressOf */
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
580 * Return : true if destination/port combination was found, false otherwise
581 * Data Used : UdpDestPortList
582 * ------------------------------------------------------------------------- */
584 InUdpDestPortList(int ip_version, union olsr_ip_addr *addr, uint16_t port)
586 struct UdpDestPort *walker;
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
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
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 * ------------------------------------------------------------------------- */
619 P2pdPacketCaptured(unsigned char *encapsulationUdpData, int nBytes)
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;
627 if ((encapsulationUdpData[0] & 0xf0) == 0x40) { //IPV4
629 ipHeader = (struct ip *) ARM_NOWARN_ALIGN(encapsulationUdpData);
631 dst.v4 = ipHeader->ip_dst;
633 if (ipHeader->ip_p != SOL_UDP) {
635 #ifdef INCLUDE_DEBUG_OUTPUT
636 OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
637 #endif /* INCLUDE_DEBUG_OUTPUT */
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 */
650 if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
653 udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData +
654 GetIpHeaderLength(encapsulationUdpData)));
655 destPort = ntohs(udpHeader->dest);
657 if (!InUdpDestPortList(AF_INET, &dst, destPort)) {
658 #ifdef INCLUDE_DEBUG_OUTPUT
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 */
666 else if ((encapsulationUdpData[0] & 0xf0) == 0x60) { //IPv6
668 ipHeader6 = (struct ip6_hdr *) ARM_NOWARN_ALIGN(encapsulationUdpData);
670 memcpy(&dst.v6, &ipHeader6->ip6_dst, sizeof(struct in6_addr));
672 if (ipHeader6->ip6_dst.s6_addr[0] == 0xff) //Multicast
676 return; //not multicast
678 if (ipHeader6->ip6_nxt != SOL_UDP) {
680 //OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
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 */
692 if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
695 udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData + 40));
696 destPort = ntohs(udpHeader->dest);
698 if (!InUdpDestPortList(AF_INET6, &dst, destPort)) {
699 #ifdef INCLUDE_DEBUG_OUTPUT
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 */
708 return; //Is not IP packet
711 // send the packet to OLSR forward mechanism
712 olsr_p2pd_gen(encapsulationUdpData, nBytes);
713 } /* P2pdPacketCaptured */
716 /* -------------------------------------------------------------------------
718 * Description: This function is registered with the OLSR scheduler and called
719 * when something is captured
724 * ------------------------------------------------------------------------- */
727 void *data __attribute__ ((unused)),
728 unsigned int flags __attribute__ ((unused)))
730 unsigned char rxBuffer[P2PD_BUFFER_SIZE];
732 struct sockaddr_ll pktAddr;
733 socklen_t addrLen = sizeof(pktAddr);
735 unsigned char *ipPacket;
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 */
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)) {
761 // "%s: captured frame too short (%d bytes) on \"%s\"\n",
762 // PLUGIN_NAME_SHORT,
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",
775 dump_packet(ipPacket, nBytes);
776 #endif /* INCLUDE_DEBUG_OUTPUT */
777 /* A multicast or broadcast packet was captured */
778 P2pdPacketCaptured(ipPacket, nBytes);
780 } /* if (pktAddr.sll_pkttype == ...) */
781 } /* if (skfd >= 0 && (FD_ISSET...)) */
784 /* -------------------------------------------------------------------------
785 * Function : InitP2pd
786 * Description: Initialize the P2pd plugin
787 * Input : skipThisInterface - pointer to interface to skip
791 * ------------------------------------------------------------------------- */
793 InitP2pd(struct interface *skipThisIntf)
796 // Initialize hash table for hash based duplicate IP packet check
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);
804 //Creates captures sockets and register them to the OLSR scheduler
805 CreateNonOlsrNetworkInterfaces(skipThisIntf);
810 /* -------------------------------------------------------------------------
811 * Function : CloseP2pd
812 * Description: Close the P2pd plugin and clean up
817 * ------------------------------------------------------------------------- */
821 CloseNonOlsrNetworkInterfaces();
824 /* -------------------------------------------------------------------------
825 * Function : SetP2pdTtl
826 * Description: Set the TTL for message from this plugin
827 * Input : value - parameter value to evaluate
830 * Data Used : P2pdTtl
831 * ------------------------------------------------------------------------- */
833 SetP2pdTtl(const char *value,
834 void *data __attribute__ ((unused)),
835 set_plugin_parameter_addon addon __attribute__ ((unused)))
837 assert(value != NULL);
838 P2pdTtl = atoi(value);
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
851 * Data Used : P2pdUseHash
852 * ------------------------------------------------------------------------- */
854 SetP2pdUseHashFilter(const char *value,
855 void *data __attribute__ ((unused)),
856 set_plugin_parameter_addon addon __attribute__ ((unused)))
858 assert(value != NULL);
859 P2pdUseHash = atoi(value);
864 /* -------------------------------------------------------------------------
865 * Function : AddUdpDestPort
866 * Description: Set the UDP destination/port combination as an entry in the
868 * Input : value - parameter value to evaluate
870 * Return : -1 on error condition, 0 if all is ok
871 * Data Used : UdpDestPortList
872 * ------------------------------------------------------------------------- */
874 AddUdpDestPort(const char *value,
875 void *data __attribute__ ((unused)),
876 set_plugin_parameter_addon addon __attribute__ ((unused)))
878 char destAddr[INET6_ADDRSTRLEN];
881 struct UdpDestPort * new;
882 struct sockaddr_in addr4;
883 struct sockaddr_in6 addr6;
884 int ip_version = AF_INET;
887 assert(value != NULL);
889 // Retrieve the data from the argument string passed
890 memset(destAddr, 0, sizeof(destAddr));
891 num = sscanf(value, "%45s %hd", destAddr, &destPort);
893 OLSR_PRINTF(1, "%s: Invalid argument for \"UdpDestPort\"",
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;
904 // Check whether the specified address was either IPv4 multicast,
905 // IPv4 broadcast or IPv6 multicast.
907 switch (ip_version) {
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... ");
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... ");
922 // Determine whether it is a valid IP address
924 OLSR_PRINTF(1, "Invalid address specified for \"UdpDestPort\"");
928 // Create a new entry and link it into the chain
929 new = calloc(1, sizeof(struct UdpDestPort));
931 OLSR_PRINTF(1, "%s: Out of memory", PLUGIN_NAME_SHORT);
935 new->ip_version = ip_version;
936 switch (ip_version) {
938 memcpy(&new->address.v6.s6_addr,
939 &addr6.sin6_addr.s6_addr,
940 sizeof(addr6.sin6_addr.s6_addr));
943 new->address.v4.s_addr = addr4.sin_addr.s_addr;
946 new->port = destPort;
947 new->next = UdpDestPortList;
948 UdpDestPortList = new;
950 // And then we're done
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
961 * Return : Pointer to the string buffer containing the result
963 * ------------------------------------------------------------------------- */
965 get_ipv4_str(uint32_t address, char *s, size_t maxlen)
967 struct sockaddr_in v4;
969 v4.sin_addr.s_addr = address;
970 inet_ntop(AF_INET, &v4.sin_addr, s, maxlen);
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
982 * Return : Pointer to the string buffer containing the result
984 * ------------------------------------------------------------------------- */
986 get_ipv6_str(unsigned char* address, char *s, size_t maxlen)
988 struct sockaddr_in6 v6;
990 memcpy(v6.sin6_addr.s6_addr, address, sizeof(v6.sin6_addr.s6_addr));
991 inet_ntop(AF_INET6, &v6.sin6_addr, s, maxlen);
996 /* -------------------------------------------------------------------------
997 * Function : is_broadcast
998 * Description: Check whether the address represents a broadcast address
999 * Input : addr - IPv4 address to check
1001 * Return : true if broadcast address, false otherwise
1003 * ------------------------------------------------------------------------- */
1005 is_broadcast(const struct sockaddr_in addr)
1007 if (addr.sin_addr.s_addr == 0xFFFFFFFF)
1013 /* -------------------------------------------------------------------------
1014 * Function : is_multicast
1015 * Description: Check whether the address represents a multicast address
1016 * Input : addr - IPv4 address to check
1018 * Return : true if broadcast address, false otherwise
1020 * ------------------------------------------------------------------------- */
1022 is_multicast(const struct sockaddr_in addr)
1024 if ((htonl(addr.sin_addr.s_addr) & 0xE0000000) == 0xE0000000)
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
1039 * ------------------------------------------------------------------------- */
1041 dump_packet(unsigned char* packet, int length)
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]);
1051 OLSR_PRINTF(1, "\n");
1053 #endif /* INCLUDE_DEBUG_OUTPUT */
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
1060 * Return : true if duplicate packet, false otherwise
1061 * Data Used : P2pdUseHash
1062 * ------------------------------------------------------------------------- */
1064 check_and_mark_recent_packet(unsigned char *data,
1065 int len __attribute__ ((unused)))
1067 unsigned char * ipPacket;
1068 uint16_t ipPacketLen;
1071 /* If we don't use this filter bail out here */
1075 /* Clean up the hash table each time before we check it */
1076 PrunePacketHistory(NULL);
1078 /* Check for duplicate IP packets now based on a hash */
1079 ipPacket = GetIpPacket(data);
1080 ipPacketLen = GetIpTotalLength(ipPacket);
1082 /* Calculate packet fingerprint */
1083 crc32 = PacketCrc32(ipPacket, ipPacketLen);
1085 /* Check if this packet was seen recently */
1086 if (CheckAndMarkRecentPacket(crc32))
1090 "%s: --> discarding: packet is duplicate\n",