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 P2pdUseTtlDecrement = 0; /* No TTL decrement by default */
89 int P2pdDuplicateTimeout = P2PD_VALID_TIME;
91 /* List of UDP destination address and port information */
92 struct UdpDestPort * UdpDestPortList = NULL;
94 /* List of filter entries to check for duplicate messages
96 struct node * dupFilterHead = NULL;
97 struct node * dupFilterTail = NULL;
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 #ifdef INCLUDE_DEBUG_OUTPUT
104 void dump_packet(unsigned char* packet, int length);
105 #endif /* INCLUDE_DEBUG_OUTPUT */
106 bool check_and_mark_recent_packet(unsigned char *data, int len);
108 /* -------------------------------------------------------------------------
109 * Function : PacketReceivedFromOLSR
110 * Description: Handle a received packet from a OLSR message
111 * Input : ipPacket into an unsigned char and the lenght of the packet
114 * Data Used : BmfInterfaces
115 * ------------------------------------------------------------------------- */
117 PacketReceivedFromOLSR(unsigned char *encapsulationUdpData, int len)
119 struct ip *ipHeader; /* IP header inside the encapsulated IP packet */
120 struct ip6_hdr *ip6Header; /* IP header inside the encapsulated IP packet */
121 struct udphdr *udpHeader;
122 struct NonOlsrInterface *walker;
123 int stripped_len = 0;
124 union olsr_ip_addr destAddr;
126 bool isInList = false;
128 ipHeader = (struct ip *) ARM_NOWARN_ALIGN(encapsulationUdpData);
129 ip6Header = (struct ip6_hdr *) ARM_NOWARN_ALIGN(encapsulationUdpData);
130 //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN got packet from OLSR message\n");
132 if (check_and_mark_recent_packet(encapsulationUdpData, len))
135 /* Check with each network interface what needs to be done on it */
136 for (walker = nonOlsrInterfaces; walker != NULL; walker = walker->next) {
137 /* To a non-OLSR interface: unpack encapsulated IP packet and forward it */
138 if (walker->olsrIntf == NULL) {
140 struct sockaddr_ll dest;
142 memset(&dest, 0, sizeof(dest));
143 dest.sll_family = AF_PACKET;
144 if ((encapsulationUdpData[0] & 0xf0) == 0x40) {
145 dest.sll_protocol = htons(ETH_P_IP);
146 stripped_len = ntohs(ipHeader->ip_len);
149 if ((encapsulationUdpData[0] & 0xf0) == 0x60) {
150 dest.sll_protocol = htons(ETH_P_IPV6);
151 stripped_len = 40 + ntohs(ip6Header->ip6_plen); // IPv6 Header size (40)
155 // Sven-Ola: Don't know how to handle the "stripped_len is uninitialized"
156 // condition, maybe exit(1) is better...?
157 if (0 == stripped_len)
160 //TODO: if packet is not IP die here
162 if (stripped_len > len) {
165 dest.sll_ifindex = if_nametoindex(walker->ifName);
166 dest.sll_halen = IFHWADDRLEN;
168 if (olsr_cnf->ip_version == AF_INET) {
169 /* Use all-ones as destination MAC address. When the IP destination is
170 * a multicast address, the destination MAC address should normally also
171 * be a multicast address. E.g., when the destination IP is 224.0.0.1,
172 * the destination MAC should be 01:00:5e:00:00:01. However, it does not
173 * seem to matter when the destination MAC address is set to all-ones
177 if (IsMulticastv4(ipHeader)) {
178 in_addr_t addr = ntohl(ipHeader->ip_dst.s_addr);
180 dest.sll_addr[0] = 0x01;
181 dest.sll_addr[1] = 0x00;
182 dest.sll_addr[2] = 0x5E;
183 dest.sll_addr[3] = (addr >> 16) & 0x7F;
184 dest.sll_addr[4] = (addr >> 8) & 0xFF;
185 dest.sll_addr[5] = addr & 0xFF;
187 /* broadcast or whatever */
188 memset(dest.sll_addr, 0xFF, IFHWADDRLEN);
190 } else /*(olsr_cnf->ip_version == AF_INET6) */ {
191 if (IsMulticastv6(ip6Header)) {
192 dest.sll_addr[0] = 0x33;
193 dest.sll_addr[1] = 0x33;
194 dest.sll_addr[2] = ip6Header->ip6_dst.s6_addr[12];
195 dest.sll_addr[3] = ip6Header->ip6_dst.s6_addr[13];
196 dest.sll_addr[4] = ip6Header->ip6_dst.s6_addr[14];
197 dest.sll_addr[5] = ip6Header->ip6_dst.s6_addr[15];
201 if (olsr_cnf->ip_version == AF_INET) {
202 // Determine the IP address and the port from the header information
203 if (ipHeader->ip_p == SOL_UDP && !IsIpv4Fragment(ipHeader)) {
204 udpHeader = (struct udphdr*) ARM_NOWARN_ALIGN((encapsulationUdpData +
205 GetIpHeaderLength(encapsulationUdpData)));
206 destAddr.v4.s_addr = ipHeader->ip_dst.s_addr;
207 #if defined(__GLIBC__) || defined(__BIONIC__)
208 destPort = htons(udpHeader->dest);
210 destPort = htons(udpHeader->uh_dport);
212 isInList = InUdpDestPortList(AF_INET, &destAddr, destPort);
213 #ifdef INCLUDE_DEBUG_OUTPUT
217 "%s: Not in dest/port list: %s:%d\n",
219 get_ipv4_str(destAddr.v4.s_addr,
224 #endif /* INCLUDE_DEBUG_OUTPUT */
226 } else /* (olsr_cnf->ip_version == AF_INET6) */ {
227 if (ip6Header->ip6_nxt == SOL_UDP && !IsIpv6Fragment(ip6Header)) {
228 udpHeader = (struct udphdr*) ARM_NOWARN_ALIGN((encapsulationUdpData + 40));
229 memcpy(&destAddr.v6, &ip6Header->ip6_dst, sizeof(struct in6_addr));
230 #if defined(__GLIBC__) || defined(__BIONIC__)
231 destPort = htons(udpHeader->dest);
233 destPort = htons(udpHeader->uh_dport);
235 isInList = InUdpDestPortList(AF_INET6, &destAddr, destPort);
236 #ifdef INCLUDE_DEBUG_OUTPUT
240 "%s: Not in dest/port list: %s:%d\n",
242 get_ipv6_str(destAddr.v6.s6_addr,
247 #endif /* INCLUDE_DEBUG_OUTPUT */
252 /* Address/port combination of this packet is not in the UDP dest/port
253 * list and will therefore be suppressed. I.e. just continue with the
254 * next interface to emit on.
259 nBytesWritten = sendto(walker->capturingSkfd,
260 encapsulationUdpData,
263 (struct sockaddr *)&dest,
265 if (nBytesWritten != stripped_len) {
266 P2pdPError("sendto() error forwarding unpacked encapsulated pkt on \"%s\"",
272 // "%s: --> unpacked and forwarded on \"%s\"\n",
273 // PLUGIN_NAME_SHORT,
276 } /* if (walker->olsrIntf == NULL) */
278 } /* PacketReceivedFromOLSR */
280 /* Highest-numbered open socket file descriptor. To be used as first
281 * parameter in calls to select(...).
283 int HighestSkfd = -1;
285 /* Set of socket file descriptors */
288 /* -------------------------------------------------------------------------
289 * Function : p2pd_message_seen
290 * Description: Check whether the current message has been seen before
291 * Input : head - start of the list to check for the message
292 * tail - end of the list to check for the message
293 * m - message to check for in the list
295 * Return : true if message was found, false otherwise
296 * Data Used : P2pdDuplicateTimeout
297 * ------------------------------------------------------------------------- */
299 p2pd_message_seen(struct node **head, struct node **tail, union olsr_message *m)
306 // Check whether any entries have aged
309 struct DupFilterEntry *filter;
310 struct node * next = curr->next; // Save the current pointer since curr may
313 filter = (struct DupFilterEntry*)curr->data;
315 if ((filter->creationtime + P2pdDuplicateTimeout) < now)
316 remove_node(head, tail, curr, true);
318 // Skip to the next element
322 // Now check whether there are any duplicates
323 for (curr = *head; curr; curr = curr->next) {
324 struct DupFilterEntry *filter = (struct DupFilterEntry*)curr->data;
326 if (olsr_cnf->ip_version == AF_INET) {
327 if (filter->address.v4.s_addr == m->v4.originator &&
328 filter->msgtype == m->v4.olsr_msgtype &&
329 filter->seqno == m->v4.seqno) {
332 } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
333 if (memcmp(filter->address.v6.s6_addr,
334 m->v6.originator.s6_addr,
335 sizeof(m->v6.originator.s6_addr)) == 0 &&
336 filter->msgtype == m->v6.olsr_msgtype &&
337 filter->seqno == m->v6.seqno) {
346 /* -------------------------------------------------------------------------
347 * Function : p2pd_store_message
348 * Description: Store a new message in the duplicate message check list
349 * Input : head - start of the list to add the message to
350 * tail - end of the list to add the message to
351 * m - message to add to the list
355 * ------------------------------------------------------------------------- */
357 p2pd_store_message(struct node **head, struct node **tail, union olsr_message *m)
361 // Store a message into the database
362 struct DupFilterEntry *new_dup = calloc(1, sizeof(struct DupFilterEntry));
363 if (new_dup == NULL) {
364 OLSR_PRINTF(1, "%s: Out of memory\n", PLUGIN_NAME_SHORT);
370 new_dup->creationtime = now;
371 if (olsr_cnf->ip_version == AF_INET) {
372 new_dup->address.v4.s_addr = m->v4.originator;
373 new_dup->msgtype = m->v4.olsr_msgtype;
374 new_dup->seqno = m->v4.seqno;
375 } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
376 memcpy(new_dup->address.v6.s6_addr,
377 m->v6.originator.s6_addr,
378 sizeof(m->v6.originator.s6_addr));
379 new_dup->msgtype = m->v6.olsr_msgtype;
380 new_dup->seqno = m->v6.seqno;
383 // Add the element to the head of the list
384 append_node(head, tail, new_dup);
387 /* -------------------------------------------------------------------------
388 * Function : p2pd_is_duplicate_message
389 * Description: Check whether the specified message is a duplicate
390 * Input : msg - message to check for in the list of duplicate messages
392 * Return : true if message was found, false otherwise
394 * ------------------------------------------------------------------------- */
396 p2pd_is_duplicate_message(union olsr_message *msg)
398 if(p2pd_message_seen(&dupFilterHead, &dupFilterTail, msg)) {
402 p2pd_store_message(&dupFilterHead, &dupFilterTail, msg);
407 /* -------------------------------------------------------------------------
408 * Function : olsr_parser
409 * Description: Function to be passed to the parser engine. This function
410 * processes the incoming message and passes it on if necessary.
411 * Input : m - message to parse
412 * in_if - interface to use (unused in this application)
413 * ipaddr - IP-address to use (unused in this application)
415 * Return : false if message should be supressed, true otherwise
417 * ------------------------------------------------------------------------- */
419 olsr_parser(union olsr_message *m,
420 struct interface_olsr *in_if __attribute__ ((unused)),
421 union olsr_ip_addr *ipaddr __attribute__ ((unused)))
423 union olsr_ip_addr originator;
426 //OLSR_DEBUG(LOG_PLUGINS, "P2PD PLUGIN: Received msg in parser\n");
428 /* Fetch the originator of the messsage */
429 if (olsr_cnf->ip_version == AF_INET) {
430 memcpy(&originator, &m->v4.originator, olsr_cnf->ipsize);
431 size = ntohs(m->v4.olsr_msgsize);
433 memcpy(&originator, &m->v6.originator, olsr_cnf->ipsize);
434 size = ntohs(m->v6.olsr_msgsize);
437 /* Check if message originated from this node.
438 * If so - back off */
439 if (ipequal(&originator, &olsr_cnf->main_addr))
440 return false; /* Don't forward either */
442 /* Check for duplicate messages for processing */
443 if (p2pd_is_duplicate_message(m))
444 return true; /* Don't process but allow to be forwarded */
446 if (olsr_cnf->ip_version == AF_INET) {
447 PacketReceivedFromOLSR((unsigned char *)&m->v4.message, size - 12);
449 PacketReceivedFromOLSR((unsigned char *)&m->v6.message, size - 12 - 96);
455 /* -------------------------------------------------------------------------
456 * Function : olsr_p2pd_gen
457 * Description: Sends a packet in the OLSR network
458 * Input : packet - packet to send in the OLSR network
459 * len - length of the packet to send
463 * ------------------------------------------------------------------------- */
465 olsr_p2pd_gen(unsigned char *packet, int len)
467 /* send buffer: huge */
470 union olsr_message *message = (union olsr_message *)buffer;
471 struct interface_olsr *ifn;
475 if ((aligned_size % 4) != 0) {
476 aligned_size = (aligned_size - (aligned_size % 4)) + 4;
480 if (olsr_cnf->ip_version == AF_INET) {
482 message->v4.olsr_msgtype = P2PD_MESSAGE_TYPE;
483 message->v4.olsr_vtime = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
484 memcpy(&message->v4.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
485 message->v4.ttl = P2pdTtl ? P2pdTtl : MAX_TTL;
486 message->v4.hopcnt = 0;
487 message->v4.seqno = htons(get_msg_seqno());
488 message->v4.olsr_msgsize = htons(aligned_size + 12);
489 memset(&message->v4.message, 0, aligned_size);
490 memcpy(&message->v4.message, packet, len);
491 aligned_size = aligned_size + 12;
492 } else /* if (olsr_cnf->ip_version == AF_INET6) */ {
494 message->v6.olsr_msgtype = P2PD_MESSAGE_TYPE;
495 message->v6.olsr_vtime = reltime_to_me(P2PD_VALID_TIME * MSEC_PER_SEC);
496 memcpy(&message->v6.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
497 message->v6.ttl = P2pdTtl ? P2pdTtl : MAX_TTL;
498 message->v6.hopcnt = 0;
499 message->v6.seqno = htons(get_msg_seqno());
500 message->v6.olsr_msgsize = htons(aligned_size + 12 + 96);
501 memset(&message->v6.message, 0, aligned_size);
502 memcpy(&message->v6.message, packet, len);
503 aligned_size = aligned_size + 12 + 96;
506 /* looping through interfaces */
507 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
508 //OLSR_PRINTF(1, "%s: Generating packet - [%s]\n", PLUGIN_NAME_SHORT, ifn->int_name);
510 if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
511 /* send data and try again */
513 if (net_outbuffer_push(ifn, message, aligned_size) != aligned_size) {
514 //OLSR_PRINTF(1, "%s: could not send on interface: %s\n", PLUGIN_NAME_SHORT, ifn->int_name);
520 /* -------------------------------------------------------------------------
521 * Function : P2pdPError
522 * Description: Prints an error message at OLSR debug level 1.
523 * First the plug-in name is printed. Then (if format is not NULL
524 * and *format is not empty) the arguments are printed, followed
525 * by a colon and a blank. Then the message and a new-line.
526 * Input : format, arguments
530 * ------------------------------------------------------------------------- */
532 P2pdPError(const char *format, ...)
534 #define MAX_STR_DESC 255
535 char strDesc[MAX_STR_DESC];
537 #if !defined REMOVE_LOG_DEBUG
538 char *stringErr = strerror(errno);
539 #endif /* !defined REMOVE_LOG_DEBUG */
541 /* Rely on short-circuit boolean evaluation */
542 if (format == NULL || *format == '\0') {
543 //OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", PLUGIN_NAME, stringErr);
547 va_start(arglist, format);
548 vsnprintf(strDesc, MAX_STR_DESC, format, arglist);
551 strDesc[MAX_STR_DESC - 1] = '\0'; /* Ensures null termination */
553 #if !defined REMOVE_LOG_DEBUG
554 OLSR_DEBUG(LOG_PLUGINS, "%s: %s\n", strDesc, stringErr);
555 #endif /* !defined REMOVE_LOG_DEBUG */
559 /* -------------------------------------------------------------------------
560 * Function : MainAddressOf
561 * Description: Lookup the main address of a node
562 * Input : ip - IP address of the node
564 * Return : The main IP address of the node
566 * ------------------------------------------------------------------------- */
568 MainAddressOf(union olsr_ip_addr *ip)
570 union olsr_ip_addr *result;
572 /* TODO: mid_lookup_main_addr() is not thread-safe! */
573 result = mid_lookup_main_addr(ip);
574 if (result == NULL) {
578 } /* MainAddressOf */
581 /* -------------------------------------------------------------------------
582 * Function : InUdpDestPortList
583 * Description: Check whether the specified address and port is in the list of
584 * configured UDP destination/port entries
585 * Input : ip_version - IP version to use for this check
586 * addr - address to check for in the list
587 * port - port to check for in the list
589 * Return : true if destination/port combination was found, false otherwise
590 * Data Used : UdpDestPortList
591 * ------------------------------------------------------------------------- */
593 InUdpDestPortList(int ip_version, union olsr_ip_addr *addr, uint16_t port)
595 struct UdpDestPort *walker;
597 for (walker = UdpDestPortList; walker; walker = walker->next) {
598 if (walker->ip_version == ip_version) {
599 if (ip_version == AF_INET) {
600 if (addr->v4.s_addr == walker->address.v4.s_addr &&
601 walker->port == port)
602 return true; // Found so we can stop here
603 } else /* ip_version == AF_INET6 */ {
604 if ((memcmp(addr->v6.s6_addr,
605 walker->address.v6.s6_addr,
606 sizeof(addr->v6.s6_addr)) == 0) &&
607 (walker->port == port))
608 return true; // Found so we can stop here
616 * Function for checksum calculation.
617 * From the RFC, the checksum algorithm is:
618 * "The checksum field is the 16 bit one's complement of the one's
619 * complement sum of all 16 bit words in the header. For purposes of
620 * computing the checksum, the value of the checksum field is zero."
622 * For example, consider Hex 4500003044224000800600008c7c19acae241e2b (20 bytes IP header):
623 * - Step 1) 4500 + 0030 + 4422 + 4000 + 8006 + 0000 + 8c7c + 19ac + ae24 + 1e2b = 0002`BBCF (16-bit sum)
624 * - Step 2) 0002 + BBCF = BBD1 = 1011101111010001 (1's complement 16-bit sum)
625 * - Step 3) ~BBD1 = 0100010000101110 = 442E (1's complement of 1's complement 16-bit sum)
627 static void recomputeIPv4HeaderChecksum(struct ip *header) {
630 u_short *headerWords;
637 nwords = header->ip_hl << 1;
638 headerWords = (u_short *) header;
641 for (sum = 0; nwords > 0; nwords--) {
642 sum += ntohs(*headerWords);
647 sum = (sum >> 16) + (sum & 0xffff);
652 header->ip_sum = htons((u_short)sum);
655 /* -------------------------------------------------------------------------
656 * Function : P2pdPacketCaptured
657 * Description: Handle a captured IP packet
658 * Input : encapsulationUdpData - space for the encapsulation header,
659 * followed by the captured IP packet
660 * nBytes - The number of bytes in the data packet
663 * Data Used : P2pdInterfaces
664 * Notes : The IP packet is assumed to be captured on a socket of family
665 * PF_PACKET and type SOCK_DGRAM (cooked).
666 * ------------------------------------------------------------------------- */
668 P2pdPacketCaptured(unsigned char *encapsulationUdpData, int nBytes)
670 union olsr_ip_addr dst; /* Destination IP address in captured packet */
671 struct ip *ipHeader = NULL; /* The IP header inside the captured IP packet */
672 struct ip6_hdr *ipHeader6; /* The IP header inside the captured IP packet */
673 struct udphdr *udpHeader;
674 uint8_t * ttl = NULL;
675 int recomputeChecksum = 0;
678 if ((encapsulationUdpData[0] & 0xf0) == 0x40) { //IPV4
680 ipHeader = (struct ip *) ARM_NOWARN_ALIGN(encapsulationUdpData);
682 dst.v4 = ipHeader->ip_dst;
684 if (ipHeader->ip_p != SOL_UDP) {
686 #ifdef INCLUDE_DEBUG_OUTPUT
687 OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
688 #endif /* INCLUDE_DEBUG_OUTPUT */
692 // If we're dealing with a fragment we bail out here since there's no valid
693 // UDP header in this message
694 if (IsIpv4Fragment(ipHeader)) {
695 #ifdef INCLUDE_DEBUG_OUTPUT
696 OLSR_PRINTF(1, "%s: Is IPv4 fragment\n", PLUGIN_NAME_SHORT);
697 #endif /* INCLUDE_DEBUG_OUTPUT */
701 if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
704 udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData +
705 GetIpHeaderLength(encapsulationUdpData)));
706 #if defined(__GLIBC__) || defined(__BIONIC__)
707 destPort = ntohs(udpHeader->dest);
709 destPort = ntohs(udpHeader->uh_dport);
712 if (!InUdpDestPortList(AF_INET, &dst, destPort)) {
713 #ifdef INCLUDE_DEBUG_OUTPUT
715 OLSR_PRINTF(1, "%s: Not in dest/port list: %s:%d\n", PLUGIN_NAME_SHORT,
716 get_ipv4_str(dst.v4.s_addr, tmp, sizeof(tmp)), destPort);
717 #endif /* INCLUDE_DEBUG_OUTPUT */
721 ttl = &ipHeader->ip_ttl;
722 recomputeChecksum = 1;
724 else if ((encapsulationUdpData[0] & 0xf0) == 0x60) { //IPv6
726 ipHeader6 = (struct ip6_hdr *) ARM_NOWARN_ALIGN(encapsulationUdpData);
728 memcpy(&dst.v6, &ipHeader6->ip6_dst, sizeof(struct in6_addr));
730 if (ipHeader6->ip6_dst.s6_addr[0] == 0xff) //Multicast
734 return; //not multicast
736 if (ipHeader6->ip6_nxt != SOL_UDP) {
738 //OLSR_PRINTF(1,"%s: NON UDP PACKET\n", PLUGIN_NAME_SHORT);
742 // Check whether this is a IPv6 fragment
743 if (IsIpv6Fragment(ipHeader6)) {
744 #ifdef INCLUDE_DEBUG_OUTPUT
745 OLSR_PRINTF(1, "%s: Is IPv6 fragment\n", PLUGIN_NAME_SHORT);
746 #endif /* INCLUDE_DEBUG_OUTPUT */
750 if (check_and_mark_recent_packet(encapsulationUdpData, nBytes))
753 udpHeader = (struct udphdr *) ARM_NOWARN_ALIGN((encapsulationUdpData + 40));
754 #if defined(__GLIBC__) || defined(__BIONIC__)
755 destPort = ntohs(udpHeader->dest);
757 destPort = ntohs(udpHeader->uh_dport);
760 if (!InUdpDestPortList(AF_INET6, &dst, destPort)) {
761 #ifdef INCLUDE_DEBUG_OUTPUT
763 OLSR_PRINTF(1, "%s: Not in dest/port list: %s:%d\n", PLUGIN_NAME_SHORT,
764 get_ipv6_str(dst.v6.s6_addr, tmp, sizeof(tmp)), destPort);
765 #endif /* INCLUDE_DEBUG_OUTPUT */
769 ttl = &ipHeader6->ip6_ctlun.ip6_un1.ip6_un1_hlim;
770 recomputeChecksum = 0;
773 return; //Is not IP packet
776 if (P2pdUseTtlDecrement) {
786 if (recomputeChecksum) {
787 recomputeIPv4HeaderChecksum(ipHeader);
791 // send the packet to OLSR forward mechanism
792 olsr_p2pd_gen(encapsulationUdpData, nBytes);
793 } /* P2pdPacketCaptured */
796 /* -------------------------------------------------------------------------
798 * Description: This function is registered with the OLSR scheduler and called
799 * when something is captured
804 * ------------------------------------------------------------------------- */
807 void *data __attribute__ ((unused)),
808 unsigned int flags __attribute__ ((unused)))
810 unsigned char rxBuffer[P2PD_BUFFER_SIZE];
812 struct sockaddr_ll pktAddr;
813 socklen_t addrLen = sizeof(pktAddr);
815 unsigned char *ipPacket;
817 /* Receive the captured Ethernet frame, leaving space for the BMF
818 * encapsulation header */
819 ipPacket = GetIpPacket(rxBuffer);
820 nBytes = recvfrom(skfd, ipPacket, P2PD_BUFFER_SIZE,
821 0, (struct sockaddr *)&pktAddr, &addrLen);
822 #ifdef INCLUDE_DEBUG_OUTPUT
823 OLSR_PRINTF(1, "%s: Received %d bytes\n", PLUGIN_NAME_SHORT, nBytes);
824 #endif /* INCLUDE_DEBUG_OUTPUT */
831 /* if (nBytes < 0) */
832 /* Check if the number of received bytes is large enough for an IP
833 * packet which contains at least a minimum-size IP header.
834 * Note: There is an apparent bug in the packet socket implementation in
835 * combination with VLAN interfaces. On a VLAN interface, the value returned
836 * by 'recvfrom' may (but need not) be 4 (bytes) larger than the value
837 * returned on a non-VLAN interface, for the same ethernet frame. */
838 if (nBytes < (int)sizeof(struct ip)) {
841 // "%s: captured frame too short (%d bytes) on \"%s\"\n",
842 // PLUGIN_NAME_SHORT,
849 if (pktAddr.sll_pkttype == PACKET_OUTGOING ||
850 pktAddr.sll_pkttype == PACKET_MULTICAST ||
851 pktAddr.sll_pkttype == PACKET_BROADCAST) {
852 #ifdef INCLUDE_DEBUG_OUTPUT
853 OLSR_PRINTF(1, "%s: Multicast or broadcast packet was captured.\n",
855 dump_packet(ipPacket, nBytes);
856 #endif /* INCLUDE_DEBUG_OUTPUT */
857 /* A multicast or broadcast packet was captured */
858 P2pdPacketCaptured(ipPacket, nBytes);
860 } /* if (pktAddr.sll_pkttype == ...) */
861 } /* if (skfd >= 0 && (FD_ISSET...)) */
864 /* -------------------------------------------------------------------------
865 * Function : InitP2pd
866 * Description: Initialize the P2pd plugin
867 * Input : skipThisInterface - pointer to interface to skip
871 * ------------------------------------------------------------------------- */
873 InitP2pd(struct interface_olsr *skipThisIntf)
876 // Initialize hash table for hash based duplicate IP packet check
880 //Tells OLSR to launch olsr_parser when the packets for this plugin arrive
881 //olsr_parser_add_function(&olsr_parser, PARSER_TYPE,1);
882 olsr_parser_add_function(&olsr_parser, PARSER_TYPE);
884 //Creates captures sockets and register them to the OLSR scheduler
885 CreateNonOlsrNetworkInterfaces(skipThisIntf);
890 /* -------------------------------------------------------------------------
891 * Function : CloseP2pd
892 * Description: Close the P2pd plugin and clean up
897 * ------------------------------------------------------------------------- */
901 CloseNonOlsrNetworkInterfaces();
904 /* -------------------------------------------------------------------------
905 * Function : SetP2pdTtl
906 * Description: Set the TTL for message from this plugin
907 * Input : value - parameter value to evaluate
910 * Data Used : P2pdTtl
911 * ------------------------------------------------------------------------- */
913 SetP2pdTtl(const char *value,
914 void *data __attribute__ ((unused)),
915 set_plugin_parameter_addon addon __attribute__ ((unused)))
917 assert(value != NULL);
918 P2pdTtl = atoi(value);
923 /* -------------------------------------------------------------------------
924 * Function : SetP2pdUseHashFilter
925 * Description: Set the Hash filter flag for this plug-in
926 * Input : value - parameter value to evaluate
927 * data - data associated with this parameter (unused in this app)
928 * addon - additional parameter data
931 * Data Used : P2pdUseHash
932 * ------------------------------------------------------------------------- */
934 SetP2pdUseHashFilter(const char *value,
935 void *data __attribute__ ((unused)),
936 set_plugin_parameter_addon addon __attribute__ ((unused)))
938 assert(value != NULL);
939 P2pdUseHash = atoi(value);
944 /* -------------------------------------------------------------------------
945 * Function : SetP2pdUseTtlDecrement
946 * Description: Set the TTL decrement lag for this plug-in
947 * Input : value - parameter value to evaluate
948 * data - data associated with this parameter (unused in this app)
949 * addon - additional parameter data
952 * Data Used : P2pdUseTtlDecrement
953 * ------------------------------------------------------------------------- */
955 SetP2pdUseTtlDecrement(const char *value,
956 void *data __attribute__ ((unused)),
957 set_plugin_parameter_addon addon __attribute__ ((unused)))
959 assert(value != NULL);
960 P2pdUseTtlDecrement = atoi(value);
965 /* -------------------------------------------------------------------------
966 * Function : AddUdpDestPort
967 * Description: Set the UDP destination/port combination as an entry in the
969 * Input : value - parameter value to evaluate
971 * Return : -1 on error condition, 0 if all is ok
972 * Data Used : UdpDestPortList
973 * ------------------------------------------------------------------------- */
975 AddUdpDestPort(const char *value,
976 void *data __attribute__ ((unused)),
977 set_plugin_parameter_addon addon __attribute__ ((unused)))
979 char destAddr[INET6_ADDRSTRLEN];
982 struct UdpDestPort * new;
983 struct sockaddr_in addr4;
984 struct sockaddr_in6 addr6;
985 int ip_version = AF_INET;
988 assert(value != NULL);
990 // Retrieve the data from the argument string passed
991 memset(destAddr, 0, sizeof(destAddr));
992 num = sscanf(value, "%45s %hd", destAddr, &destPort);
994 OLSR_PRINTF(1, "%s: Invalid argument for \"UdpDestPort\"",
999 // Check whether we're dealing with an IPv4 or IPv6 address
1000 // When the string contains a ':' we can assume we're dealing with IPv6
1001 if (strchr(destAddr, (int)':')) {
1002 ip_version = AF_INET6;
1005 // Check whether the specified address was either IPv4 multicast,
1006 // IPv4 broadcast or IPv6 multicast.
1008 switch (ip_version) {
1010 res = inet_pton(AF_INET6, destAddr, &addr6.sin6_addr);
1011 if (addr6.sin6_addr.s6_addr[0] != 0xFF) {
1012 OLSR_PRINTF(1,"WARNING: IPv6 address must be multicast... ");
1017 res = inet_pton(AF_INET, destAddr, &addr4.sin_addr);
1018 if (!is_broadcast(addr4) && !is_multicast(addr4)) {
1019 OLSR_PRINTF(1,"WARNING: IPv4 address must be multicast or broadcast... ");
1023 // Determine whether it is a valid IP address
1025 OLSR_PRINTF(1, "Invalid address specified for \"UdpDestPort\"");
1029 // Create a new entry and link it into the chain
1030 new = calloc(1, sizeof(struct UdpDestPort));
1032 OLSR_PRINTF(1, "%s: Out of memory", PLUGIN_NAME_SHORT);
1036 new->ip_version = ip_version;
1037 switch (ip_version) {
1039 memcpy(&new->address.v6.s6_addr,
1040 &addr6.sin6_addr.s6_addr,
1041 sizeof(addr6.sin6_addr.s6_addr));
1044 new->address.v4.s_addr = addr4.sin_addr.s_addr;
1047 new->port = destPort;
1048 new->next = UdpDestPortList;
1049 UdpDestPortList = new;
1051 // And then we're done
1055 /* -------------------------------------------------------------------------
1056 * Function : get_ipv4_str
1057 * Description: Convert the specified address to an IPv4 compatible string
1058 * Input : address - IPv4 address to convert to string
1059 * s - string buffer to contain the resulting string
1060 * maxlen - maximum length of the string buffer
1062 * Return : Pointer to the string buffer containing the result
1064 * ------------------------------------------------------------------------- */
1066 get_ipv4_str(uint32_t address, char *s, size_t maxlen)
1068 struct sockaddr_in v4;
1070 v4.sin_addr.s_addr = address;
1071 inet_ntop(AF_INET, &v4.sin_addr, s, maxlen);
1076 /* -------------------------------------------------------------------------
1077 * Function : get_ipv6_str
1078 * Description: Convert the specified address to an IPv4 compatible string
1079 * Input : address - IPv6 address to convert to string
1080 * s - string buffer to contain the resulting string
1081 * maxlen - maximum length of the string buffer
1083 * Return : Pointer to the string buffer containing the result
1085 * ------------------------------------------------------------------------- */
1087 get_ipv6_str(unsigned char* address, char *s, size_t maxlen)
1089 struct sockaddr_in6 v6;
1091 memcpy(v6.sin6_addr.s6_addr, address, sizeof(v6.sin6_addr.s6_addr));
1092 inet_ntop(AF_INET6, &v6.sin6_addr, s, maxlen);
1097 /* -------------------------------------------------------------------------
1098 * Function : is_broadcast
1099 * Description: Check whether the address represents a broadcast address
1100 * Input : addr - IPv4 address to check
1102 * Return : true if broadcast address, false otherwise
1104 * ------------------------------------------------------------------------- */
1106 is_broadcast(const struct sockaddr_in addr)
1108 if (addr.sin_addr.s_addr == 0xFFFFFFFF)
1114 /* -------------------------------------------------------------------------
1115 * Function : is_multicast
1116 * Description: Check whether the address represents a multicast address
1117 * Input : addr - IPv4 address to check
1119 * Return : true if broadcast address, false otherwise
1121 * ------------------------------------------------------------------------- */
1123 is_multicast(const struct sockaddr_in addr)
1125 if ((htonl(addr.sin_addr.s_addr) & 0xE0000000) == 0xE0000000)
1131 #ifdef INCLUDE_DEBUG_OUTPUT
1132 /* -------------------------------------------------------------------------
1133 * Function : dump_packet
1134 * Description: Dump the specified data as hex output
1135 * Input : packet - packet to dump to output
1136 * length - length of the data in the packet
1140 * ------------------------------------------------------------------------- */
1142 dump_packet(unsigned char* packet, int length)
1146 OLSR_PRINTF(1, "%s: ", PLUGIN_NAME_SHORT);
1147 for (idx = 0; idx < length; idx++) {
1148 if (idx > 0 && ((idx % 16) == 0))
1149 OLSR_PRINTF(1, "\n%s: ", PLUGIN_NAME_SHORT);
1150 OLSR_PRINTF(1, "%2.2X ", packet[idx]);
1152 OLSR_PRINTF(1, "\n");
1154 #endif /* INCLUDE_DEBUG_OUTPUT */
1156 /* -------------------------------------------------------------------------
1157 * Function : check_and_mark_recent_packet
1158 * Description: Wrapper function for the Hash based duplicate check
1159 * Input : data - pointer to a packet of data to be checked
1161 * Return : true if duplicate packet, false otherwise
1162 * Data Used : P2pdUseHash
1163 * ------------------------------------------------------------------------- */
1165 check_and_mark_recent_packet(unsigned char *data,
1166 int len __attribute__ ((unused)))
1168 unsigned char * ipPacket;
1169 uint16_t ipPacketLen;
1172 /* If we don't use this filter bail out here */
1176 /* Clean up the hash table each time before we check it */
1177 PrunePacketHistory(NULL);
1179 /* Check for duplicate IP packets now based on a hash */
1180 ipPacket = GetIpPacket(data);
1181 ipPacketLen = GetIpTotalLength(ipPacket);
1183 /* Calculate packet fingerprint */
1184 crc32 = PacketCrc32(ipPacket, ipPacketLen);
1186 /* Check if this packet was seen recently */
1187 if (CheckAndMarkRecentPacket(crc32))
1191 "%s: --> discarding: packet is duplicate\n",